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ABSTRACT 


The  major  problem  addressed  by  this  research  is  how  to  aUow  an  autonomous  vehicle 
to  dynamically  recognize  changes  in  its  environment,  to  mi^  its  environment,  and  alto-  its 
path  to  avoid  obstacles  while  still  reaching  its  goal  point 

The  approach  takoi  was  to  modify  existing  sonar  functions  in  previous  work,  to  better 
utilize  sonars,  and  to  perform  many  experiments  to  determine  what  data  to  expect  from 
sonars  while  the  vehicle  is  in  motion.  By  applying  the  linear  square  fitting  algorithm,  the 
robot  has  the  ability  to  map  the  objects  within  sensor  range  of  an  autonomous  vehicle. 

The  results  are  that,  given  an  initial  and  goal  point,  the  robot  can  proceed  on  a  directed 
path,  utilize  its  sonar  sensor(s)  used  to  detect  obstacles,  and  whoi  an  obstacle  is  detected 
have  the  capability  to  dynamically  compute  a  parallel  path  and  smoothly  alter  its  motion  to 
the  parallel  path.  The  robot  now  has  the  capal^ty  to  track  the  obstacle,  and,  once  clear  of 
the  obstacle  smoothly  alter  its  motion  to  a  path  that  will  reach  its  goal  point  The  ability  for 
the  robot  to  combine  smooth  motion  widi  obstacle  avoidance  has  now  been  successfully 
programmed. 
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I.  INTRODUCTION 


A.  BACKGROUND 

Obstacle  recognition  is  a  common  application  of  sonars  used  in  many  differait 
q)plications.  Motion  control  and  path  following  for  autonomous  vehicles  can  be 
accomplished  in  a  variety  of  ways.  Whether  or  not  the  vehicle  is  in  a  static  or  changing 
world  will  drastically  alter  the  padi  it  will  take.  In  previous  research  at  die  Naval 
Postgraduate  School,  obstacles  detected  by  sonars  woe  addressed  by  Solomon  Sheifey  in 
[Ref.  1  ]  and  resulted  in  an  ability  to  use  sonars  that  was  not  easily  acconqilished  from  a  user 
standpoint  Sonars  are  used  to  deteimifM  location,  and  to  recognize  obstacles  to  be  avoided. 
A  high  level  language  called  MML  (model  bases  mobile  robot  language)  is  the  driving 
force  behind  the  robot  Yamabico.  Real  testing  of  software  development  and  algorithms  can 
be  done  on  an  autonomous  vehicle.  By  restructuring  the  code  and  making  it  more  modular, 
this  will  make  MML  moxt  portable  with  the  ability  to  q>ply  its  functions  to  odier  vehicles. 

B,  OVERVIEW 

Although  Yamabico  nuy  have  precise  knowledge  of  its  location  in  a  given 
environment,  it  is  only  cqrable  of  detecting  the  presence  of  unexpected  obstacles  in  its  path 
by  relying  on  its  12  sonars  that  can  be  operated  at  anytime.  They  have  an  accuracy  of 
approximately  1  centimeter,  and  are  consistrat  in  their  results.  Yamabico  can  move  at  a 
q>eed  up  to  65  centimeters  per  second  in  a  translational  motion,  forwards  or  backwards. 
The  sonars  are  low  to  the  ground  and  will  not  pick  up  obstacles  that  are  high,  such  as 
overhangs,  or  low  obstacles  below  38  coitimeters.  If  an  obstacle  is  detected,  YanuAico  has 
tile  ability  to  alt»  its  path  to  avoid  the  obstacle,  and  once  clear  of  the  obstacle,  return  to  its 
original  path.  Oice  the  obstacle  is  recognized,  Yamabico  should  smoothly  transition  to  an 
alternative  path  and  smootiily  transition  back  to  tiie  original  path  once  it  can  be  done  safely. 
Hence,  tiie  fusion  of  an  obstacle  being  recognized  and  then  having  motion  functions 
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available  to  smoothly  deviate  from  the  original  planned  padi  are  the  basic  premises  cm 
which  tire  sonar  system  for  Yamabico  has  been  designed  and  implemented. 
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n.  PROBLEM  STATEMENT 


The  problems  we  are  addressing  can  be  brokoi  into  two  basic  components. 

1.  Tlw  first  is  to  construct  a  sonar  function  library,  with  a  simple,  transparent  and 
efficient  user  interfietce. 

2.  The  second  component  is  to  use  these  functions  for  real  time  obstacle  avoidance.  The 
sonar  system  can  be  used  concurrently  with  the  vehicle  in  motion,  either  translational  or 
rotational.  The  sonars  can  be  used  to  determine  whoe  an  obstacle  is,  and  for  a  vehicle  to 
dynamically  transition  from  its  pre-planned  padi  to  anodier  newly  planned  path  in  order  to 
maintain  the  safest  path  while  avoiding  obstacles. 

A.  SONAR  FUNCTION  LIBRARY 

One  of  the  problems  in  past  sonar  functions  has  been  that  they  were  logically  incorrect 
and  unreliable.  The  C  code  was  not  written  in  ANSI  C,  so  a  lot  of  checking  at  compilation 
for  such  things  as  type  and  parameter  matching  being  two  examples,  was  never  being  done. 
A  strong  asset  of  ANSI  C  is  that  it’s  the  next  step  towards  C++,  which  then  gives  us  die 
benefit  of  object  oriented  programming  with  inheritance. 

The  first  step  in  improving  sonar  functions  is  to  trace  key  algorithms  to  ensure  diatthey 
are  being  properly  implemented,  and  to  ensure  that  all  unnecessary  code  is  removed.  Over 
time,  adjustments  have  been  made  to  different  functions,  with  needless  variables  and 
equations  not  being  deleted.  Later,  more  complex  functionality,  such  as  linear  square 
fitting,  was  reorganixed. 

The  second  step  in  improving  sonar  functions  is  to  make  diem  more  modular.  That  is 
to  shorten  the  sonar  code  into  smaller  functions  that  are  correct  and  easily  understood. 
Making  a  module  small  to  perform  one  basic  operation  for  each  is  one  step  towards  making 
the  code  more  portable.  By  making  the  functions  easily  understood,  the  user  should  have 
no  problem  implementing  the  functions,  and  testing  them  on  an  actual  platform. 
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B.  OBSTACLE  AVOIDANCE 


Consider  a  vehicle  travosing  a  hallway  on  a  safest  path  trajectory,  say  the  cmter  of  the 
hallway.  Along  the  way  the  vehicle  encounters  an  obstacle  in  its  path.  In  order  to  continue 
on  a  safest  path  trajectory,  the  vehicle  must  change  its  linear  path.  This  functionality  is 
possible  with  the  linear  fitting  and  some  other  basic  capabilities. 

The  introduction  of  obstacle  avoidance  as  a  function  that  can  be  called  when  planning 
the  path  of  the  vehicir  is  one  way  to  give  intelligence  to  the  robot  in  navigating  a  path. 

One  simple  way  to  do  this  is  to  (see  Figure  1)  detect  the  obstacle  with  a  forward 
looking  sonar,  shift  to  a  parallel  path,  and  then  resume  its  original  path  once  the  side 
detecting  sonar  detects  no  obstacle.  This  demonstrates  the  ability  of  a  vehicle  to 
dynamically  alter  its  path  for  a  safer  path,  and  resume  its  original  path  once  the  obstacle  has 
passed. 


parallel  path 


original  path 

obstacle 


Figure  1:  Example  of  obstacle  avoidance 

Writing  functions  that  allow  the  vehicle  to  intelligently  avoid  obstacles  and  reach  its 
goal  as  the  world  becomes  more  complex  will  be  the  approach  taken.  It  is  important  to 
ensure  each  function  is  sound  in  solving  one  problem,  and  that  difierent  functions  can  be 
dynamically  used  as  diffoent  situations  arise. 


4 


As  a  path  becomes  more  complex,  and  the  number  of  obstacles  increases,  additional 
decision  making  becomes  necessary.  The  challrage  will  be  to  cover  as  many  different  cases 
as  possible,  and  to  dynamically  path  plan  in  a  timely  manner. 
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m.  SONAR  HARDWARE  SYSTEM 


A.  HARDWARE  SYSTEM 

Yamabico’s  sonar  hardware  is  extremely  efficient  because  a  dedicated  sonar  board 
with  a  microprocessor  controls  the  sonar  sensors.  Yamabico’s  nuun  central  processing  unit 
is  interrupted  only  when  data  becomes  available  from  the  sonar  array.  The  sonar  syston 
provides  user  interface  functions  that  control  Yamabico's  array  of  sonar  range  finders.  At 
any  point  within  a  user’s  program,  any  of  the  12  sonars  nuiy  be  enabled  or  disabled.  This 
allows  the  user  to  operate  a  given  sonar  only  when  necessary  for  a  particular  ^plication. 
When  needed,  the  sonar  system  returns  the  latest  reading  of  a  specified  sonar  out  of  the 
twelve.  This  system  design  is  far  better  than  the  primitive  one  in  which  a  usa  must  wait  30 
milliseconds  after  he/she  issues  a  command.  A  user’s  program  can  also  be  forced  to  “busy 
wait”  until  some  sonar-based  condition  is  satisfied.  This  feature  is  particularly  valuable  for 
obstacle  avoidance.  For  example,  a  user’s  program  could  be  written  to  wait  until  the 
forward  looking  sonar’s  range  is  less  than  distance  d,  then  stop.  A  block  diagram  of  the 
sonar  system  is  provided  in  Hgure  2. 


Figure  2;  Yamabico  sonars 
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Hgme  3  shows  the  cuirait  hardware  configuratioii  of  Yamabico. 


1.  Sonar  Grouping 

In  order  to  reduce  sampling  time  the  sonars  are  operated  in  logical  groups  of 
four.  The  sonars  of  a  logical  group  are  all  pulsed  simultaneously  and  thus  the  san^Ung  time 
is  reduced  by  a  factor  of  four  as  compared  to  individual  firing  of  the  sonars.  The  sonars  of 
each  logical  group  are  oriented  in  such  a  way  as  to: 

-  prevent  mutual  interference 

-  provide  a  “look”  in  all  four  directions  from  each  group 

-  present  a  similar  aspect  from  each  sonar  during  a  rotational  scan 

Thus,  logical  group  0  consists  of  sonars  0,  2,  S  and  7  (see  Figure  2),  group  1 
consists  of  sonars  1, 3, 4  and  6;  group  2  consists  of  sonars  8, 9, 10  and  1 1;  and  group  3  is  a 
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“virtual”  group  which  consists  of  four  permanent  test  values.  The  sonars  of  a  group  are 
symmetric  about  the  robot’s  axis  of  rotation. 

In  addition  to  being  logically  grouped,  the  sonars  are  also  physically  grouped 
(see  Figure  3).  The  physical  grouping  of  the  sonars  is  made  to  distribute  the  electrical  load 
over  the  driver  boards  evenly  and  thus  minimize  any  electrical  transients  associated  with 
operation  of  the  sonar.  The  physical  grouping  connects  sonars  0,  2,  8  and  11  to  driver/ 
amplifier  board  1;  sonars  4,  S,  6  and  7  to  board  2;  and  sonars  1, 3, 9  and  10  to  board  3.  The 
reader  will  note  that  pairs  of  sonars  from  logical  groups  are  assigned  to  physical  groups,  for 
example,  sonars  0  and  2  from  logical  group  0  are  assigned  to  physical  group  (driver/ 
amplifier  board)  1. 

Initial  design  of  die  control  circuitry  was  based  on  two  primary  parameters:  (1) 
a  desired  maximum  range  of  400  centimeter,  and  (2)  a  pulse  width  of  1  millisecond. 
Assuming  a  speed  of  sound  in  air,  at  sea  level,  of  340  meters/second  we  may  calculate  a 
round-trip  time: 

round  trip  time  «  x  2  ■  2333  msec  (Eq  3.1) 

This  round  trip  tune  is  the  period  during  which  a  valid  echo  may  be  received  and 
is  referred  to  as  the  receive  gate.  This  intoval  is  rounded  up  to  24  milliseconds  and  is 
derived  by  division  of  the  sonar  system’s  2  MHz  clock  to  ensure  diat  the  receiver  is  not 
falsely  triggered  by  a  direct  path  reception  from  it’s  adjacent  transmitter,  we  opt  to  disable 
the  receiver  until  the  transmit  pulse  is  conqilete.  This  will  have  the  disadvantage  of  setting 
a  minimum  range  equal  to  half  the  distance  sound  would  travel  in  the  time  of  a  transmit 
pulse. 

minimum  range  «  34000  cmVsec.  x  1  msec,  x  0.S  s  17  cm.  (Eq  3.2) 

This  minimum  range  lies  approximately  9  centimeters  outside  the  periphery  of 
the  robot  In  order  to  allow  the  measurement  of  objects  up  to  the  periphery  of  the  robot  the 
pulse  width  was  decreased  to  0.5  milliseconds  thus  reducing  the  minimum  range  to  8.5 
centimeters. 
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In  actual  practice,  the  minimum  range  is  set  by  firmware  to  9.6  coitimetars,  the 
additional  distance  being  due  to  time  allotted  for  switching  and  settling  in  the  circuitry. 

All  sonars  of  a  logical  group  are  pulsed  simultaneously.  Which  groups  are  fired 
is  determined  by  the  value  of  the  corresponding  bit  in  the  command  register  of  the  sonar 
control  board,  which  in  tom  is  set  by  the  user  with  an  MML  function  (see  Figure  3).  Hence, 
if  bit  2  is  set  to  1  then  group  2  sonars  will  be  pulsed.  If  more  than  one  group  is  selected  to 
be  pulsed,  the  sonar  control  board  will  pulse  the  first  group  on  the  list,  and  when  the  data 
from  that  pulse  has  been  read  from  the  fourth  data  regista  the  sonar  control  board  will 
proceed  to  the  next  group  and  pulse  it,  and  so  on  in  round  robin  fashion.  Groups  with  their 
control  bit  set  to  0  will  not  be  pulsed.  The  sampling  rate  can  thus  be  as  high  as  41  Hz  with 
only  one  group  enabled  (based  on  a  24  millisecond  read  gate  as  determined  in  Equation  3.2) 
and  will  be  halved  for  each  additional  group  enabled.  At  a  nominal  robot  speed  of  30 
centimeters  per  second,  this  sampling  rate  could  provide  an  updated  range  within  0.75 
centimeter  of  travel,  exceeding  our  desired  positional  accuracy  of  1  cent '..f  er.  Of  course, 
real  performance  will  be  affected  by  any  delay  in  reading  die  data  registers  due  to  other 
demands  on  the  central  processor  (processing  the  sonar  data,  controlling  motion,  etc.). 

2.  Range  Finding 

There  are  four  16  bit  data  registers  on  the  sonar  control  board,  one  for  each  of 
the  four  sonars  in  a  logical  group.  When  the  transmit  pulse  is  sent  to  the  driver/amplifier 
boards  a  counter  is  started  which  increments  each  of  the  data  registers  every  6 
microseconds.  This  time  period  is  equivalent  to  a  range  of  1.02  millimeto:: 

lange  «  340000  mm/secx  6  microsec  xO.S  =  1.02  mm  (Eq3.3) 

The  incrementing  of  a  particular  data  register  continues  until  an  echo  is  received 
or  the  range  gate  times  out  The  first  12  bits  of  the  data  register  are  allotted  for  range 
accumulation,  thus  allowing  for  a  maximum  range  of  4.177  meters  (4095  x  1.02  mm.).  If 
the  range  gate  should  time  out  before  an  echo  is  received,  the  high  bit  of  the  over  ranged 
sonar’s  data  register  is  set  to  1.  This  is  the  “over  range”  bit  and  is  used  to  signal  the  ensuing 
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software  that  no  echo  was  received.  Bits  12, 13  and  14  of  the  data  registers  are  not  used. 
When  the  ranging  cycle  is  complete,  the  appropriate  group  number  is  written  into  bits  4  and 
5  of  the  status  register  and  the  “ready”  bit,  bit  7  of  the  status  register,  is  set  to  1.  The  ready 
bit  is  used  as  a  flag  when  operating  in  the  polled  mode;  i.e.  without  intemq)ts. 

3.  Interrupt  Control 

The  sonar  control  board  is  actually  a  daughtercard  which  rides  on  a  VME  bus 
mothercard.  The  mothercard  carries  address  decoders,  bus  drivers  and  interrupt  control 
circuitry  in  the  Bus  Interface  Module  (BIM). 

When  the  sonar  has  completed  a  ranging  cycle  an  interrupt  request  is  provided 
to  the  BIM.  The  BIM’s  control  register  holds  information  which  determines  whetha  an 
interrupt  is  to  be  genmted  or  not,  and  if  so  which  interrupt  level  is  to  be  generated. 
Presuming  an  interrupt  is  generated,  when  the  correct  acknowledgment  returns  on  the 
address  lines  the  BIM’s  vector  register  provides  the  vector  table  entry  where  the  central 
processor  may  find  the  vector  to  the  interrupt  handler.  The  correct  interrupt  level,  the 
interrupt  enable  bit  and  interrupt  vector  are  loaded  to  the  BIM  during  software 
initialization. 

4.  Data  Transfer 

Each  of  the  data  registers  is  individually  addressed  on  the  VME  bus  by  a  VME 
short  address,  as  is  the  status  register.  Transferral  of  die  data  is  extremely  straightforward. 
The  interrupt  handler  simply  reads  the  correct  register,  masks  out  the  unwanted  bits  and 
writes  the  data  to  the  stack.  When  the  last  data  register  is  read,  the  sonar  systnn  resets  the 
data  registers  and  commences  a  ranging  cycle  on  the  next  soiuur  group  in  it’s  round  robin. 
The  system  will  continue  to  operate  autonomously  until  all  the  sonars  are  disabled. 
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IV.  BASIC  SONAR  FUNCTIONS 


A.  DISTANCE 

There  are  two  functions  available  to  return  sonar  values.  One  function,  stmarQ  will 
return  the  range  from  the  sonar  to  die  object  it  is  getting  the  return  from.  If  tfiere  is  no  return, 
then  a  value  of  infinity  is  assigned,  and  for  Yamabico  diis  value  is  999999.  The  infinity 
value  is  used  for  trouble  shooting  purposes,  to  cfetect  whether  or  not  there  are  instances  of 
no  return  from  objects  at  a  distance  of  less  dian  4  meters.  The  second  range  function 
available  is  globalQ,  and  this  will  return  the  x,y  coordiruues  of  where  the  return  was 
detected  in  the  world  that  the  vehicle  is  in.  This  is  useful  in  the  vehicle  making  a  mq>  of  its 
world  with  obstacles  in  it  These  functions  can  be  found  in  AppoMlix  A. 

B.  GLOBAL  POSITION  CALCULATIONS 

By  utilizing  the  con^se  function  described  by  [Ref.  3]  and  seen  in  Hgure  4,  we  can 
determine  the  actual  point  in  a  2D  coordinate  system.  Let  the  following  equations  rqxresent 
q/  anq2. 


(Eq4.1) 


q^m 


(Eq4.2) 


The  composition  of  these  transfomutions  is  defined  as 


^ x^  -t-XjCMOj  - 
y^  -f  jCjUnOj  jjcmGi 


®l^®2 


(Eq4.3) 
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Figure  4:  Composition 


This  functionality  is  extremely  usefully  in  dynamically  configuring  new  paths  from  your 
original  paths.  You  can  dynamically  define  another  path  dq)ending  on  your  position  and 
the  direction  of  your  vehicle.  For  the  sonar  functions,  it  allows  much  more  modularity  to 
the  code.  The  code  is  reusable,  since  the  only  thing  unique  to  Yamabico  are  the  actual  sonar 
positions  on  die  robot 
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y.  LINEAR  FEATURE  EXTRACTION 


In  addition  to  simple  range  and  point  position  data,  die  sonar  systnn  recognizes  the 
linear  features  of  an  orthogonal  world.  To  do  so  we  must  provide  some  method  for 
recognizing  sets  of  data  points  which  form  die  linear  feature  and  a  method  for  finding  and 
describing  the  line  segment  that  best  fits  that  set  of  data  points.  This  is  accomplislwd  in 
reverse  fashion,  i.e.  we  presume  the  data  we  are  receiving  belongs  to  such  a  set  and 
continuously  modify  a  descriptive  line  segment  to  a  best  fit  of  the  data  using  a  least  squares 
fitting  algorithm.  This  line  segment  continues  to  grow  until  die  incoming  data  or  certain 
measures  of  the  line  segment  indicate  that  die  line  segment  should  be  ended  and  a  new  one 
started.  We  use  an  implementation  of  least  squares  fitting  described  by  [Ref.  1]. 


A,  LEAST  SQUARES  FITTING 

Suppose  we  have  collected  n  consecutive  valid  data  points  in  a  local  coordinate 
system,  (p/,...,  Pn),  where  p,-  =  (x,-,  yj)  for  i  =  l,...,n.  We  obtain  the  moments  nijg^  of  the  set 
of  points 

m 

i0^j,k^2,anij*k^2)  (EqS.l) 

i-l 

Notice  that  mgo  »  n.  The  centroid  C  is  given  by 


C 


The  secondary  moments  around  the  centroid  are  given  by 


i-l 


«3j- 


(Eq5.2) 


(Eq5.3) 


M 


II 


1-1 


-  «„ 


W|0<»«0l^ 
«oo  / 


(Eq5.4) 
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(Eq5.5) 


<-l 


•fhn- 


(Wqi)* 

••w 


We  adopt  die  parametric  representadon  (r,a)  of  a  line  with  constants  r  and  a.  If  a  point 
P  *  Cx»y)  satisfies  an  equation 

r  ■  zcosa-fysina  (-ic/2<a^x/2)  (EqS.6) 

then  the  point  p  is  on  a  line  L  whose  normal  has  an  orientation  a  and  whose  distance  from 
the  origin  is  r  (Hgure  S).  This  method  has  an  advantage  in  expressing  lines  diat  are 
popoidicular  to  the  X  axis.  The  point-slope  method,  where  y  -  mx  +  b,ii  inciqiable  of 
representing  such  a  case  is  undefined). 


Figure  5:  Representation  of  a  line  L  using  rand  a 

The  residual  of  point  pf-  -  (x,-,  y|)  ami  die  line  L  =  (r,a)  is  z,cosa-i'y,sina-r.  Tha:^oie, 
the  sum  of  die  squares  of  all  residuals  is 

S  ■  21)  (Eq5.7) 

<-i 

The  line  which  best  fits  the  set  of  points  is  supposed  to  minimize  S.  Thus  the  optimum 
line  (r,a)  must  satisfy 
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(Eq5.8) 


^-^-0 

ir  40, 

Thus, 


■ 

y  “  25^(r-x,c«o->iilno) 


'  i-i  '<-1  -  ^i-i  '  ' 


2  {rm^  -  Mjocosa  -  «oi  »hia) 


(Eq5.9) 


(EqS.lO) 


(Eq5.11) 


**I0  **01  . 

r  a  — =ooca-f  — sina  >  M.^co>a-»-^-sina 
**00  **00 

where  r  may  be  negative.  Substituting  r  in  Equation  (5.7)  by  Equation  (5.12) , 


(Eq5.12) 


S  »  2((x,-^,)C08O+  (y,-u,)smo)5 


(Eq5.13) 


Finally, 

m 

^  ■  2^^  ( (Xj - u,) cosa ♦  (ji - M,)  sino)  (-  (x, - ^,)  sina *  {y^- |i,) cwa)  (Eq  5.14) 

“  2  (y, -»!,)*-  (Xj-H,)*)8lnoco80+22  (x.-ji,)  (y.-M^)  (co«*a-8iiFa)(Eq  5.15) 


(Sfgj  -  M]o)  sin2a  •*>  2M„  co82a 


(Eq5.16) 


Therefore 


(2M||/  (M^— Mjg)) 


(Eq  5.17) 


Equation  (5.12)  and  Equation  (5.17)  are  the  solutions  for  die  line  parameters 
gmerated  by  a  least  squares  fit 
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B.  FINDING  ENDPOINTS 

The  residual  of  a  point  p,- « (Xj.  >,)  is 

8,  ■  (Py-y,)8ino  (EqS.18) 

Therefore,  the  projection,  p\  of  the  point  p,-  onto  the  major  axis  is 
p'i  ■  (atj  ♦  8, COSO,  Vi  +  5,sina)  (Eq  5.19) 

We  will  use  p\  and  p\  as  estimates  of  the  em^ints  of  the  line  segment  L  obtained 
from  tile  setp  of  data  points. 

C.  RESIDUAL  TESTING 

We  wish  to  do  some  pre-filtering  of  the  data  in  order  to  remove  points  from  tiie  data 
stream  which  are  clearly  not  colinear  with  the  existing  points  of  set  p.  In  this  way  we  can 
often  detect  the  end  of  a  line  segment  before  having  to  perform  the  considerable 
computations  necessary  to  include  it  in  tiie  line.  If  the  point  satisfies 

8j^i<iiux(oxCl,C2)  (EqS.20) 

where  Cl  and  C2  are  positive  constants  (typically.  Cl  «  0.02  and  C2  «  S.O)  ti^n  die  point 
can  be  included  in  tiie  current  line  segment  C2  at  S.O  allows  for  mote  residual  at  a  distance 
greats  than  250  centimeters,  up  to  8  centimeters  at  a  distance  of  4  meters. 

D.  BEGINNING  LINE  SEGMENTS 

First  the  sonar  returns  must  fall  within  their  physical  constraints.  For  Yamabico, 
accqitable  return  values  fall  between  9.3  coitimeters  and  409  centimeters.  If  a  sonar  return 
is  not  within  this  range,  a  segment  wiU  be  genoated  if  there  have  been  at  least  10  previous 
returns  that  met  all  requirements  of  tiie  least  square  fitting  to  qualify  as  a  segment 

Secondly,  if  it  is  the  first  return,  you  simply  store  it  as  the  starting  point  and  proceed 
with  the  n«t  retum. 

With  the  line  segment  established,  collection  and  testing  of  the  additional  data  points 
can  proceed.  If  the  data  point  passes  the  residual  testing,  the  moments  and  test  values  for 
the  line  are  calculated  including  the  new  point  Should  that  test  pass,  the  line  segmmt 
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param^ers  (endpoints,  length,  etc.)  are  updated  and  the  system  proceeds  to  gadier  a  new 
data  point 

E.  ENDING  LINE  SEGMENTS 

There  are  three  ways  in  which  a  line  segmrat  is  ended.  It  may  be  ended  by  die  failure 
of  data  points  to  pass  the  residual  testing,  explicitly  ended  by  the  sonar  being  disabled,  or 
by  die  sonar  return  being  outside  the  acceptable  range. 
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VI.  MML  USER  INTERFACE 


A  GLOBAL  CALCULATION 

The  compose  function  is  implemented  in  a  sonar  function  called  calculate_global.  It 
applies  the  coiTq>ose  function  twice.  The  first  time  the  compose  function  is  used  to 
determine  the  actual  position  of  the  sonar  in  die  world  being  navigated  by  the  vehicle,  as 
seen  in  Hgure  6.  In  this  example  Yamabico  is  at  coordinates  (80,40),  in  the  “world 
coordinates’*.  The  sonars  position  on  the  robot  is  (9.5,  -19.75).  By  iq;>plying  the  conqiose 


Figure  6:  Example  of  first  compose 
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function. 


^ X|  -t'XjOMdj 
y, +Xjiine, +yjCOie, 


£^♦02 


we  detennine  the  position  of  the  sonar  in  “worid  coordinates”.  In  this  case  it  would  be: 
world  sonar  x  coordinate  »  100.68  s  80  +  9.S*cos(ic/4)  -  (-19.7S*sin(ii/4)) 
world  sonar  y  coordinate  =  32.74  =  40  +  9.5*sin(jt/4)  +  (-19.75*cos(ic/4)) 
world  sonar  theta  =  -k/4  =  it/4  +  -(ii/2). 

The  second  time  compose  is  t^lied  it  determines  where  the  sonar  return  is  in  the  world 
being  navigated  by  the  robot,  as  in  Figure  7. 

In  this  case  we  t^ly  the  compose  function  and  die  results  are: 
sonar  x  coordinate  from  robot  =  171.42  s  100.68  -i-  35*cos(-ji/4)  •  0*sin(-]i/4) 
sonar  y  coordinate  from  robot  =  -37.94  =  32.74  +  35*sin(-j^4)  +  0*cos(-ii/4) 
which  gives  us  the  point  in  Figure  8. 

By  knowing  where  each  sonar  is  on  the  vehicle  (see  Figure  9  and  Table  1)  and  knowing 
where  the  vehicles  position  is,  we  can  consistently  determine  where  the  object  being 
detected  is  in  relation  to  the  world  that  Yamabico  is  in.  This  is  needed  so  that  a  vehicle  can 
dynamically  map  out  the  world. 
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5 


36  cm 

Figure  9:  Sonar  positions  on  Yamabico 
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'nible  1:  SONAR  POSITIONS 


Sonar 

X 

y 

e 

0 

18  cm 

9.5  cm 

0.0 

1 

-  18  cm 

9.5  cm 

x 

2 

-  18  cm 

-9.5  cm 

X 

3 

18  cm 

-9.5  cm 

0.0 

4 

9.5  cm 

19.75  cm 

X/2 

5 

-9.5  cm 

19.75  cm 

X/2 

6 

-9.5  cm 

-19.75  cm 

-X/2 

7 

9.5  cm 

-19.75  cm 

-X/2 

8 

-15  cm 

19.75  cm 

3/4  X 

9 

-14.5  cm 

-19.75  cm 

-3/4  X 

10 

15.5  cm 

-19.75  cm 

-X/4 

11 

16  cm 

19.75  cm 

x/4 

B.  SONAR  FUNCTIONS 

Sonar  functions  are  found  in  sonarcardx,  sonarmathx,  sonaiio.c,  sonarsysx,  and 
sonarlog.c,  which  are  part  of  Yamabico’s  MML  (model  based  mobile  robot  language),  the 
name  for  the  entire  set  of  code  for  Yamabcio.  The  following  functions  and  all  sonar  code 
can  be  found  in  Appendix  A.  The  following  are  tiiose  functions  which  are  available  for  use 
in  the  user.c  and  a  brief  description. 


1.  Enable  Sonar 
Syntax:  void  enable_linear_fltting(n) 
intn; 

Description: 
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The  user  calls  tfiis  function  passing  in  the  sonar  that  is  to  be  enaUed.  On 
Yanutbico  there  are  12  available  sonars.  Each  sonar  should  be  oiabled  individually. 

2.  Disable  Sonar 
Syntax:  void  disable_sonar(n) 
intn; 

Description: 

The  user  calls  this  function  passing  in  the  sonar  that  is  to  be  disabled.  On 
Yamabico  there  are  12  available  sonars.  Each  sonar  should  be  enabled  individually. 

3.  Get  Sonar  Returns 
Syntax:  double  sonar(n) 
intn; 

Description: 

The  user  calls  this  function  and  passes  in  the  sonar  number  that  range  data  is 
wanted  from.  If  no  echo  is  received,  then  an  INFINITY(1.0e6)  is  returned.  If  the  distance 
is  less  dian  10  cm,  then  a  0  is  returned.  If  the  sonar  return  is  between  9  cm  to  409  cm,  then 
that  floating  point  number  will  be  returned  in  centimeters. 

4.  Get  Global  Sonar  Returns 

Syntax:  posit  global(n) 
intn; 

Description: 

The  user  calls  this  function  and  passes  in  the  sonar  number  that  global  range  data 
is  wanted  from.  The  function  will  return  a  structure  of  type  posit,  which  contains  gx  and  gy, 
the  global  x  and  y  coordinates. 
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5.  Enable  Lincur  Fitting 
Syntax;  void  enable.linear_|itting(n) 
intn; 

Description: 

The  user  calls  this  function  and  passes  in  the  sonar  number,  so  that  linear  fitting 
is  applied  to  sonar  returns.  This  will  enable  the  robot  to  determine  v^ietha  sonar  returns 
are  walls,  or  some  type  of  linear  surface. 

6.  Disable  Linear  Fitting 
Syntax:  void  disable_linear_fitting(n) 
intn; 

Description: 

The  user  calls  this  function  and  passes  in  the  sonar  that  linear  fitting  is  to  be 
disabled  on. 

7.  Set  Parameters  In  Linear  Square  Fitting 
Syntax:  void  set_sonar_paraincters(cl,  c2) 

float  cl,c2; 

Description: 

Allows  the  user  to  adjust  constants  which  control  the  linear  fitting  algorithm  Cl 
is  a  multiplier  to  allow  more  leniency  for  greater  sotuur  ranges,  and  C2  will  adjust  the 
tolerance  allowed  for  sonar  ranges  being  off  the  linear  line  being  collected.  Bodi  are  used 
to  determine  if  an  individual  data  point  is  usable  for  the  algorithm  The  default  values  are 
initialized  to  0.02  and  5.0  reqtectively.  For  more  information  on  Cl  and  C2  refer  to  Quitter 
V.C  of  this  thesis. 

8.  Enable  Data  Logging 

Syntax: 
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void  enable_data^logging(n4iletype^enumber) 
int  n,filetype,lijleniiinber; 

Descri|Mion: 

The  user  calls  diis  function  and  passes  in  the  sonar,  die  type  of  file  data  to  be 
collected,  and  which  file  array  (0, 1, 2,  or  3)  to  collect  the  data  in.  There  are  three  types  of 
file  data  that  can  be  collected.  The  first  is  nw  data,  the  second  is  global  data,  and  die  third 
is  segment  data. 

9.  Disable  Data  Logging 

Syntax:  void  disable_data_logging(n,filetype) 

int  n,  filetype 

Description: 

The  user  calls  this  function  and  passes  in  the  sonar,  the  type  of  file  data  to 
collected,  and  which  file  array  (0,  1,  2,  or  3).The  type  of  file  data  that  is  to  cease  being 
collected  is  designated,  either  raw  data,  global  data,  or  segment  data. 

10.  Set  Logging  interval 
Syntax:  void  set_log_interval(n,d) 
int  n,  d; 

Descrqition: 

The  user  caUs  this  function  passing  an  integer  designating  how  often  the  sonar 
data  being  collected  should  be  written  to  the  file  collecting  the  data.  The  default  value  is 
13,  which  for  a  qieed  of  30  centimeters  per  second  and  sonar  sampling  time  of  25 
milliseconds,  would  record  a  data  point  api^oximately  every  10  cm.  To  collect  all  soiur 
data  you  pass  in  1,  so  that  every  sonar  return  is  recorded. 

11.  Transfer  Raw  Data  To  Host 

Syntax:  void  xferjraw_to_host(filenunti)a,filename) 
int  filenumber,  filraame; 
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Description: 

The  user  calls  this  function  and  passes  in  the  file  number  (0, 1, 2,  tv  3)  and  the 
name  of  die  file  that  is  to  be  created  at  die  workstation  to  contain  the  raw  sonar  data 
collected. 

12.  Transfer  Global  Data  To  Host 

Syntax:  void  xfer_global_to_host(filenumber.fflename) 
int  filenumber,  filename; 

Description: 

The  user  calls  this  function  and  passes  in  the  file  number  (0, 1. 2,  or  3)  and  the 
name  of  the  file  that  is  to  be  created  at  the  woricstation  to  contain  die  global  sonar  data 
collected. 

13.  Transfer  Segment  Data  To  Host 

Syntax:  void  xfer_segment_to_host(filenumber,  filename) 
int  filenumber,  filename: 

Description: 

The  user  calls  this  function  and  passes  in  the  file  number  (0, 1, 2,  or  3)  and  the 
name  of  the  file  that  is  to  be  created  at  the  workstation  to  contain  the  segment  sonar  data 
collected. 

C.  DATA  LOGGING  PROCEDURE 

After  Yamabico  has  completed  its  mission,  recorded  sonar  data  can  be  downloaded  and 
checked  to  ensure  that  die  hardware  is  performing  optiiiully.  The  data  that  can  be  logged 
includes  global  sonar  data,  raw  sonar  data,  segment  sonar  data,  and  the  motion  trace  data 
of  the  robot  Once  the  robot  has  stopped,  the  data  designated  to  be  logged  in  user.c  can  now 
be  downloaded.  A  message  on  the  powerbcok  will  instruct  the  user  to  connect  the  phone 
cable  to  the  robot  Once  the  phone  line  is  connected,  the  user  must  hit  the  space  bar,  thoi 
the  characta  g,  and  the  space  bar  once  more.  The  data  will  then  be  downloaded  to  the 
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workstation.  Once  tlie  download  is  completed,  a  bell  sound  will  be  heard  firom  the 
powerbotdc  on  the  robot  This  is  required  for  each  type  of  data  being  logged. 
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VIL  SONAR  CHARACTERISTICS  EXPERIMENTAL  RESULTS 


To  be  able  to  successfully  use  sonars  in  path  navigation  and  obstacle  avoidance,  it  is 
necessary  to  understand  what  data  you  can  expect  in  different  situations  using  sonars.  This 
way  you  can  determine  in  which  cases  you  will  be  able  to  successfully  avoid  obstacles,  and 
in  which  cases  you  will  be  unable  to  determine  a  safe  path  with  only  input  from  the  sonars. 

A.  CASEl 

The  robot  is  moving  using  its  right  sonar  in  a  translational  scan  as  in  Figure  10.  You 
would  expect  to  get  very  accurate  data  and  to  be  able  to  recognize  the  waU.  As  Figure  1 1 
and  Hgure  12  show,  this  is  the  case.  As  expected,  the  robot  can  determine  that  there  is  a 
wall,  and  sonar  returns  have  an  accuracy  within  one  centimeter. 


Wall 

Sonar 

- - ^ 

Figure  10:  Case  1 
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1280  1300  1320  1340  1360  1380  1400  1420  1440  1460 

Figure  11:  Global  and  segment  sonar  data  from  translational 

scan 


Figure  12:  Local  trace  and  segmoit  sonar  data  brom 
translational  scan 
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B.  CASE  2 

The  robot  is  moving  using  its  sonar  in  a  translational  scan  and  transfers  to  a  line  90 
degrees  from  its  starting  line  at  a  comer  as  in  Figure  13. 


The  results  can  be  found  in  Hgure  14  and  Figure  IS.  The  sonar  can  accurately 
detect  both  waUs,  with  a  45  degree  segment  produced  for  the  comer. 
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C.  CASE  3 


The  third  case  is  the  robot  in  a  translational  scan  of  a  comer  and  the  actual  way  it 
perceives  it  surroundings  as  in  Figure  16.  You  would  expect  the  results  would  not 
accurately  reflect  the  comer  due  to  the  amount  of  reflection,  and  the  poor  angle  to  get 
returns  off  the  wall.  This  is  the  case  with  the  sonar  not  detecting  the  walls  close  to  the  point 
where  they  meet  at  a  90  degree  angle,  as  shown  in  Figure  17  and  Figure  18. 
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Figure  17:  Path  and  global  sonar  returns 


Figure  18:  Path,  comer,  and  sonar 
segment  data 
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D.  CASE  4 

The  fourth  case  is  the  robot  in  a  rotational  scan  as  in  Hgure  19  with  a  horizontal  wall. 
With  this  type  of  obstacle  you  would  expect  that  the  detection  rate  would  be  good.  Tests 
results  have  shown  that  the  robot  is  able  to  recognize  the  wall  using  the  linear  square  Htting 
algorithm  with  line  segments,  with  the  robot  rotating,  as  seen  in  Figure  20  and  Figure  21. 
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E.  CASES 


The  fifth  case,  seen  in  Figure  22  is  the  robot  in  a  rotational  scan  with  two  walls  forming 
a  90  degree  angle.  The  results  are  similar  to  those  in  case  2,  with  a  translational  scan.  The 
results  can  be  seen  in  Figure  23  and  Figure  24 


Figure  22:  Case  5 
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F.  CASE  6 


The  sixth  case,  seen  in  Hgure  25  is  a  rotational  scan  with  2  walls  forming  a  point  In 
this  case  there  were  no  sonar  returns  in  the  testing  that  was  done,  as  would  be  expected  due 
the  poor  angle  of  return  from  the  walls  back  to  the  sonar. 


G.  CASE? 

In  this  case,  seen  in  Figure  26  we  have  a  circular  object  and  for  testing  purposes  a 
plastic  can  with  a  SS  centimetn  diameter  and  height  of  70  centimetm  was  used.  The 
purpose  was  to  test  how  difficult  it  would  be  to  recognize  a  circular  object  In  a  translational 
scan,  the  circular  object  was  very  accurately  detected.  The  global  sonar  returns  in  Figure 
27  show  the  curvature  of  the  object  When  testing  for  a  segment  we  are  able  to  detect  a  line 
segment  from  the  obstacle,  which  will  assure  the  ability  to  nuq>  the  obstacle  that  is  thoe. 
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FicurelS:  Sonar  segment  and  trace 

H.  CASES 

In  this  case,  seen  in  Hgure  29  we  have  a  circular  object,  and  for  testing  purposes  a 
plastic  can  with  a  55  centimeter  diameter  and  height  of  70  centimeters  was  used.  The 
purpose  was  to  test  how  difficult  it  would  be  to  recognize  a  circular  object  in  a  rotational 
scan.  The  results  ate  shown  in  Figure  30  and  Figure  31.  The  obstacle  is  detected  but  not  as 
accurately  as  in  the  translational  scan. 


Figure  30:  Sonar  global  and  trace 
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Figure  31:  Sonar  segment 
and  trace  data 
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Vin.  OBSTACLE  AVOIDANCE  UTILIZING  SONARS 


A.  OBSTACLE  AVOIDANCE 

Given  that  you  have  a  starting  point,  and  a  finishing  point,  you  should  be  able  to  reach 
your  goal  even  if  there  is  an  obstacle  in  your  path.  We  will  start  out  with  simple  cases  that 
are  easily  resolved  by  the  vehicle.  The  starting  point  and  goal  point  will  be  on  one  line,  with 
the  robot  transiting  to  a  parallel  line  when  an  obstacle  is  detected.  When  the  compose 
function  previously  discussed  is  used,  it  is  possible  to  designate  a  parallel  line  to  the  left  or 
the  right  of  Yamabico*s  current  path,  with  the  user  either  designating  the  distance  between 
the  two  paths  or  using  a  side  sonar  to  determine  the  distance  of  the  parallel  padi.  Using  the 
compose  function  to  compute  a  parallel  path  allows  Yamabico  to  dynamically  alter  its  path. 

1.  Detecting  Obstacle  With  No  Depth 

This  is  the  simplest  case.  There  is  a  starting  point  and  a  goal  point  on  a  linear 
line.  There  is  a  small  obstacle.  There  is  no  depth  to  die  obstacle,  and  once  the  object  is 
detected,  die  vehicle  will  shift  to  a  parallel  line  to  avoid  the  obstacle,  and  then  shift  back  to 
its  original  path  (see  figure  32).  The  general  assimiptions  made  in  this  case  are  that  there  is 
room  for  the  vehicle  to  maneuver  to  a  parallel  line,  that  there  is  at  most  one  obstacle  and 
that  there  is  no  depth  to  the  obstacle.  For  this  case  only  one  forward  looking  sonar  will  be 
necessary  to  detect  the  obstacle. 

To  detect  a  small  obstacle,  the  vdiicle  moves  forward  with  its  forward  sonar.  If 
an  obstacle  is  detected,  the  vehicle  will  maneuver  to  a  parallel  line  with  a  specified  distance 
from  the  original  line,  left  or  right  of  the  obstacle.  Which  way  the  vehicle  turns  to  avoid  the 
obstacle  is  left  to  the  user.  The  user  can  use  side  sonars  to  determine  which  side  will  give 
the  robot  greater  freedom  to  maneuver.  Or  for  example  he  can  have  a  heuristic  that  anytime 
an  obstacle  is  detected  by  a  forward  sonar  that  he  will  shift  to  a  parallel  line  to  the  right 
The  distance  to  shift  can  be  a  simple  heuristic,  for  example  shift  to  a  parallel  line  one  meter 
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^orig-(^i  •yi>^i>^i)  obstacle 
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^ paraUel-(^2>  3'2*  ^l) 


Figure  32:  lyaversal  around  an  Obstacle 

distance  to  the  original  path,  or  it  can  be  determined  using  side  sonars,  for  example  taking 
a  range,  subtracting  50  centimeters  from  it  and  then  shift  that  distance.  A  sample  pseudo¬ 
code  program  for  obstacle  avoidance  is  as  follows: 
definejine(xj,  yj,  0;,  Kj,  &pl) 
follow Jine(  &pl ) 

if(sonar_detects Jdrwardjobstacle)  then 
define _parallelJine(X2,  y2, 0/.  K/,  &p2) 
shiftjto j)aralleljine(&p2) 
shiftJo_origJine(Xj,  yj.  Qj,  K;,  &pl) 
endif 

Figure  33  shows  that  the  robot  has  tracked  a  line  Y  =  ISO  with  a  goal  configuration 
(1650,150)  and  0  degree  orientation.  The  robot  opens  its  front  sonar  while  it  is  tracking  on 
its  current  path,  as  soon  as  the  distance  from  an  obstacle  is  less  than  100  centimeters,  it 
transitions  to  an  avoidance  path  which  is  line  Y  =  50.  When  the  robot  passes  the  obstacle, 
it  returns  to  its  original  path  after  traveling  past  the  obstacle  for  two  meters,  and  stops  at  its 
final  goal  configuration.  This  can  be  done  with  a  minimal  number  of  commands.  The  user.c 
file  used  to  direct  the  robot’s  mission  can  be  found  in  Appendix  B. 
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Figure  33:  Obstacle  avoidance  results 

2.  ejecting  Obstacle  With  Depth 

The  second  case  will  assume  that  the  obstacle  will  have  depth,  and  that  it  is  thin 
enough  to  maneuver  around.  There  is  a  start  and  a  finish  on  a  linear  line.  The  depth  of  the 
obstacle  is  unknown,  so  the  vehicle  will  need  to  determine  that  it  is  safe  to  resume  its 
original  path  once  the  obstacle  is  clear  (see  figure  34).  The  general  assumptions  made  in 
this  case  are  that  there  is  room  for  it  to  maneuver  to  a  parallel  line,  and  there  is  at  most  one 
obstacle.  For  this  case,  one  forward  looking  sonar,  and  side  looking  sonars  will  be 
necessary  to  detect  the  obstacle  and  to  detect  that  it  is  clear  of  the  obstacle. 

To  do  this,  the  vehicle  is  set  in  motion  with  a  forward  sonar  and  side  sonars  on. 
Once  an  obstacle  is  detected,  the  vehicle  will  shift  to  a  parallel  line  on  its  left  or  right  as 
long  as  it  is  safe  to  do  so.  If  there  is  an  obstacle  detected  with  the  side  sonars,  the  vehicle 
will  have  to  determine  whether  or  not  there  is  enough  room  to  clear  the  forward  obstacle 
and  side  obstacle.  In  our  case  with  Yamabico,  perhaps  we  will  maneuver  to  a  line  one  meter 
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ftom  the  obstacle  detected.  The  robot  should  maneuver  to  the  parallel  line  with  the  most 
room.  If  there  is  an  obstacle  detected  within  some  cut  off  range  on  both  sides,  the  vehicle 
will  stop  and  wait  for  further  instructions. 


^orig  =(Xi,  yj.  0;.  Kj)  obstacle 


^paraUel~(^2’  ^2'  ®7'  ^l) 


Figure  34:  Traversal  around  an  Obstacle  with 
Depth 

Once  the  vehicle  transitions  to  the  parallel  line,  it  will  detect  the  obstacle  it  has 
shifted  lines  to  avoid.  Once  the  vehicle  sonar  no  longer  detects  the  obstacle,  it  will  shift 
back  to  its  original  line  and  continue  towards  its  goal  point  The  pseudo-code  program  for 
obstacle  avoidance  is  as  follows: 

d^nejine(xi,  yj.  0/,  K;,  &pl) 
follow Jine(  &pl ) 

ifisonar  detects Jorward_obstacle)  then 
define _parallel_line(x2, 
shift  Jo parallel  line(&p2) 
while(sonarjletectsj}bstacle_at_side) 
remain  on  paralleljine 
endwhile 

shift  Jo j)rigjine(xi,  yi>  ^i> 
endif 
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Yamabico  can  use  sonar  as  its  environimntal  sensors  to  execute  this  obstacle  avoidance 
missions.  Figure  35  shows  that  the  robot  has  tracked  a  line  Y  =  0  with  a  goal  configuration 
(500,0)  and  0  degree  orientation.  The  robot  opens  its  front  sonar  while  it  is  tracking  on  its 
current  path,  as  soon  as  the  distance  from  an  obstacle  is  less  than  1(X)  centimeters,  it 
transitions  to  an  avoidance  path  which  is  line  Y  =  -100  and  opens  the  side  sonar  to  detect 
the  obstacle  until  it  passes  the  obstacle.  When  the  robot  passes  the  obstacle,  it  returns  to  its 
original  path  and  stops  at  its  final  goal  configuration.  This  can  be  done  with  a  minimal 
number  of  commands.  The  user.c  frle  used  to  direct  the  robot’s  mission  can  be  found  in 
Appendix  B. 
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K.  CONCLUSION 


A.  RESULTS 

Yamabico's  sonar  function  library  for  the  sonar  system  is  now  complete.  It  accurately 
applies  all  algorithms  and  the  results  are  very  accurate.  The  linear  square  fitting  algorithm 
is  accurately  applied  to  sonar  data  returned  from  walls  to  build  line  segments  that  will 
reflect  the  wall.  The  sonars  now  use  the  compose  function  to  compute  the  global  position 
of  sonar  returns.  The  user  can  employ  a  select  numba  of  functions  to  utilize  the  sonars  in 
obstacle  avoidance. 

Basic  sonar  characteristics  taken  by  translational  scanning  and  rotational  scanning 
showed  very  reasonable  results  for  the  sonars.  The  experiments  taken  and  results  indicate 
that  there  is  a  high  degree  of  accuracy  using  the  sonars  while  the  robot  is  moving. 

Simple  obstacle  avoidance  is  a  success.  The  motion  system  and  sonar  system 
coordination  is  a  success.  Testing  has  shown  Uuu  the  motion  functions  and  sonar  functions 
can  be  jointly  used  to  successfully  detect  an  obstacle,  and  dynamically  alter  its  path  to  avoid 
the  obstacle.  The  coordination  is  perfect  between  the  sonar  and  motion  systems. 

B.  RECOMENDATIONS 

Some  of  the  12  sonars  have  been  upgraded  and  hence  are  more  accurate  than  othos. 
The  front  four  sonars  are  the  most  recently  replaced  sensors.  The  renuuning  sonars  need 
hardware  upgrading,  and  testing  should  be  done  to  see  ensure  sonars  are  woridng  optimally. 

For  better  and  more  complex  avoidance,  the  use  of  a  parabola  is  needed  in  the 
locomotion  functionality.  This  will  improve  the  transition  firom  one  path  to  another  path, 
and  allow  more  complex  motion  when  avoiding  an  obstacle.  Follow  on  work  needs  to 
ensure  that  the  motion  and  sonar  systems  continue  to  woric  together. 
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APPENDIX 


This  appendix  contains  the  C  code  for  all  sonar  functions  and  for  the  user  files  that 
generaced  the  results  found  in  this  thesis. 

A.  SONAR  CODE 
/* 

Author  :  Patrick  Byrne 
Date  :  22  February  1994 
Hie  :  sonarcard.h 

Description  :  Provides  exton  declarations  for  functions  in  sonarcard.c 

*/ 

extern  void  enable.interrupt.operationO; 
extern  void  disable.interrupt.operationO; 
extern  void  enable_linear_fittingO; 
extern  void  disable_linear_fittingO; 
extern  void  enable.sonarQ; 
extern  void  disable.sonarQ; 
extern  void  scrvc_sonarO; 
extern  double  wait_sonar(); 
extern  void  reset_momentsO; 

*  Author  :  Patrick  Byrne 

*  Date  :  22  February  1994 
*File 

*  sonarcard.c 

*  Description  :  Provides  the  following  functions  for  the 

*  Sonar  Interface  card  in  sonarcard.c: 

*  void  enable_intemipt_operationO;  void  disable.intemipt.operationO;  void 

*  enable_linear_fittingO;  void  disable_iinear_fitting();  void 

*  enable_sonarO;  void  disable.sonarQ;  void  serve_sonarO;  double 

*  wait_sonar();  void  reset_momentsO;  void  wMt_until(); 

* 

♦/ 

#include  “nunl.h” 

#ifdefSIM 

#include“/n/geniiniAvork2/yamabico/mnil/Sim/spatial.h” 

#endif 
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Z******************************************************* 

*  Procedure:  enable_sonar(n) 

*  Description:  oiables  the  sonar  group 

*  that  contains  sonar  n,  which  causes  all  die  sonars  in  that  group  to 

*  echo-range  and  write  data  to  the  data  registers  on  die  sonar 

*  control  board.  Marks  the  n’th  position  of  the  enabled.sonars  array 
to  track  which  sonars  are  enabled. 


*4t4i*4>**4i4i*******4i************«**4i*4i****4i****4i4i4i4i4i«4i**««********4i**«^ 


void 

enable_sonar(n) 
int  n; 

{ 


#ifhdefSIM 
int  i; 
i  =  imaskoffO; 

#endif 

enabled_sonars[n]  =  1; 

switch  (n)  { 

caseO: 

case  2: 

case  S: 

case?: 

#ifhdefSIM 

enabled  =  enabled  1 0x01; 

#else 

sonar_group[0]  =  ON; 

#endif 

break; 

case  1: 
case  3: 
case  4: 
case  6: 

#ifndefSIM 

enabled  »  enabled  1 0x02; 

#else 

sonar_group[l]  =  ON; 

#endif 

break; 

case  8: 
case  9: 
case  10: 
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case  11: 

#ifiidefSIM 

enabled  «  enabled  1 0x04; 

#else 

sonar_group[2] «  ON; 

#endif 

break; 
case  12: 
case  13: 
case  14: 
case  IS: 

enabled  =  oiabled  1 0x08; 
break; 

} 

#ifndef  SIM 

*command_ptr  s  enabled; 
imaskon(i); 

#endif 

} 


y******************************************************************* 

* 

*  Procedure:  disable_sonar(n) 

*  Description:  removes  the  sonar  n 

*  from  the  enabled_sonars  list  If  sonar  n  is  the  only  enabled  sonar 

*  from  it’s  group,  then  the  group  is  disabled  as  well  and  will  stop 

*  echo  ranging.  This  has  benefit  of  riiortening  the  ping  interval  for 

*  groups  diat  ronain  enabled. 

4i*4i*4i4i*4i***4i**4i*4i*4i*4i*4i«**4i4i**4i***4i*****4i**4i*4i4i4i******4i4i«i**********y 


void 

disable_sonar(n) 
int  n; 

{ 

int  i,  c; 

#ifndefSIM 
i  =  imaskoffO; 

enabled_sonars[n]  =  0; 
#endif 

switch  (n)  { 
caseO: 
case  2: 
case  S: 
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case?: 

c  s  enabled_soiiars[0]  enaUed_sonars[2]  + 
enable(Lsoiiars[S]  +  enaUed_sonars{7]; 
if(c««0) 

#ifhdefSIM 

enabled  »  enabled  A.  Oxfe; 

#else 

sonar_group[0]  =  OFF; 

#endif 

break: 

case  1: 
case  3: 
case  4: 
cased: 

c  s  enabled_sonars[l]  +  enabled_sonars[3]  + 
enabled_sonars[4]  +  enabled_sonars[6]; 
if(c»=0) 

#ifndefSIM 

enabled  »  enabled  &  Oxfd; 

#else 

sonar_group[l]  =  OFF; 

#endif 

break; 

case  8: 
case  9: 
case  10: 
case  11: 

c  =  enabled_sonars[8]  +  enabled_sonars[9] 

enabled_sonars[10]  +  enabled_sonars[l  1]; 
if(c  =  0) 

#ifndef  SIM 

enabled  =  enabled  &  Oxfb; 

#else 

sonar_group[2]  s  OFF; 

#endif 

break; 
case  12: 
case  13: 
case  14: 
case  IS: 

c  s  enabled_jonars[12]  +  enabled_sonars[13]  + 
enabled_sonars[14]  +  «iabled_sonars[15]; 
if  (c  ss  0) 


iifndefSIM 

enabled  «  enaUed  &  Oxf7; 

#eLse 

sonar_^iip[3]  ■  OFF; 

#endif 

break; 

} 

#ifhdefSIM 

*comniand_ptr  «  enabled; 
iinaskon(i); 

#endif 

} 

z******************************************************************* 

*  Procedure:  wai(_sonar(n) 

*  Descr^tion:  waits  in  a  loop  until  new  data  is  available  for 

*  sonar  n. 

****  *«*********«*«««««**«********•**•**•**********•«***•••*•******/ 


double 

wait_sonar(n) 
int  n; 

{ 

sonar_table[n].update  »  0; 
while  (sonar_table[n].update  ~  0); 
return  sonar_table[n].d; 

} 


z******************************************************************* 

*  Procedure:  enable_linear_fitting(n) 

*  Description:  causes  the  background  system  to  gather  data  points 

*  from  sonar  n  and  form  them  into  line  segments  as  governed  by 

*  the  linear  fitting  algorithm.  Increments  sorvice.flag. 

************4i«****«*«****«4i*****4i*4r*4i4i**********4i*«****4i*4i4i**4i4i****Z 


void 

enable_linear_fitting(n) 
int  n; 

{ 


sonar_table[n].fitting  =  1; 
sonar_table[n].global » 1; 
++service_flag; 
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} 


/* 


7 


/*  Procedure:  disaUe_linear_fitting(n) 

*  Description:  causes  background  system  to  cease  forming  line 

*  segmoits  for  sonar  n. 

*  Decrements  the  service_flag.  Will  also  disable  the  calculation  of 

*  global  coordinates  for  that  sonar  if  data  logging  of  global  data  is 


*  not  enabled. 

*****«*****4i*********************«*********4i**********************4i/ 


void 

disable_linear_fitting(n) 
int  n; 

{ 

generatB_segment(n); 
sonar_table[n]iittuig  »  0; 
if  (sonar_table[n].filetype[l] »  0) 
sonar_tid)le[n].global »  0; 
-service_flag; 

} 


y******************************************************************* 

*  Procedure:  enable_inteiTupt_operationO 

*  Description:  places  sonar 

*  control  board  in  intenupt  driven  mode. 

******4i*4i«*******************4i*************************************/ 


void 

enable.intemipt.opCTationO 

{ 

*BIM_ptr  =  *BIM_ptr  1 0x10; 

} 


/* 


*  Procedure:  disable_inteiTupt_operationO  Description:  stops  intmupt 

*  generation  by  the  sonar  control  board.  A  flag  is  set  in  the  status 

*  register  whoi  data  is  ready,  and  it  is  the  user’s  responsibility  to  poll 

*  the  sonar  system  for  die  flag. 


4i4i4i*4i*«*4i*4i4i**4i4i*4i4i********«*****4>4i4i****************************4i^ 


void 
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disable JntBTTupUopentionO 
*BIM_ptr  a  *BIM_ptr  A,  Oxef; 

} 


#ifndefSIM 


y****************************************************** 

*  Ptocedure:  serve_sonar(x,y,t,ovfl,datal,data2,data3/iata4,groiq>) 

*  Description:  this  procedure  is  the  ^'central  command’*  for  the 

*  control  of  all  sonar  related  functions.  It  is  linked  with  die 


*  ih_sonar  routine  and  loads  sonar  data  to  die  sonar.taUe  from 

*  there.  It  then  examines  the  various  control  flags  in  die 

*  sonar.table  to  determine  which  activities  the  user  wishes  to  take 

*  place,  and  calls  the  i^ipropriate  functions.  This  procedure  is 

*  invoked  iqiproximately  every  thirty  milliseconds  by  an  interrupt 

*  from  the  sonar  control  board. 


***************4i***»*****4i«***************************************/ 


void 

serve_sonar(x.  y,  t,  ovfl,  data4,  dataS,  data2,  datal,  group) 
double  X,  y,  t; 

int  ovfr,  data4,  data3,  data!,  datal,  group; 

I 

int  n; 

int  i; 

int  data[4]; 

int  ovfl_mask  =  8; 


data[0]  s  datal 
data[l] «  data2 
data[2] »  dataS 
data[3] »  data4 


for  (i  =  0;  i  <  4;  i++,  ovfl_mask  /=  2)  { 
n  s  group_atiay[group][i]; 
if  (ovfl_mask  &  ovfl)( 

sonar_table[n].d  =  INFINITY; 

} 

else  { 

sonar_table[n].d  » (double)  data[i]  / 10.0; 

} 
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s(»iar_tatde{n].x  «  x; 
sonar_taUe[n].y  «  y; 
sonar_table(ii].t « t; 
s<Miar_taUe{n].updatB  « 1; 

} 

if  (service.flag !»  0)  ( 

fw  (i »  0;  i  <  16;  i++)  { 

if  (soiiar_table[i].updiue  »  1)  { 

if  (sonar_u^le[i].globtd  « 1) 
calciilatB_globalQ); 
if  (sofiar_table[i].fitting  » i) 
linear_fitting(i); 

if  (sonar_table[i].filetype[0] » 1) 

log_da^i,  1.  sonar_table[i].filenuii4)er[0],  0); 
if  (sonar_table[i].filetype[l]  i) 

log_data(i,  2,  sonar_taUe[i].filenuiiibei(l],  0); 

} 

soiur_table[i].update  »  0; 

} 

} 

) 

#else 


****««********4i**4i************4>*****)ii******************************* 

Procedure:  serve_soiiar(w,  group) 

Description:  this  procedure  is  the  Simulator  “central  command”  for  the 
control  of  all  sonar  related  functions.  It  then  examines  the  various 
sonar.table  to  determine  which  activities  the  user  wishes  to 
control  flags  in  the  take  place,  and  calls  the  q>propriate  functions. 

This  procedure  is  invok^  every  third  ping. 


void 

setve_sonar(w,  group) 

World  *w; 
int  group; 

{ 

} 

#ifdefjij 

Line.segment  sonar.line; 
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Line.segment  wall; 

Point  p.  q,  pi,  ql; 

Polygon  *cuiTent_poly; 

Vertex  *current_vertcx; 

int  i; 

int  j; 

int  k; 

int  n; 

double  sonar_line_theta: 

dooMe  wall..theta; 

douUe  sonar.theta; 

/*  for  each  sonar  in  the  group  being  served  */ 
for(i  =  0;i<4;i-H-){ 

n  «  group_array[group][i]; 

/*  save  the  robot  posture  */ 

sonar_table[n].x  -  vehicle.x; 

sonar_table[n].y  »  vehicie.y; 

sonar_table[n].t «  vehicle.t; 

if  ((sonar_group[group])  &&  (enabled  sonars[n]))  { 

/* 

*  ptintf(“%s%dSn’V’Sonar  groiq)  firing  =>  group); 

*/ 

sonar_table{n].d  =  INFINITY; 

/*  define  the  sonar  beam  *! 

p.x  «  vdiicle.x  +  (cos(vehicle.t  +  sonar_table[n].phi)  ♦ 

sonar_table[n].offset); 

p. y  *  vehicle,y  +  (sin(vehicle.t  +  sonar_table[n].phi)  * 

sonar_table[n].offset); 

q. x  =  pji  +  (cos(vehicle.t  -t-  sonar_table[n].axis)  *  410.0); 
q.y  ~  p.y  -»■  (sin(vehicle.t  +  sonar_table[n].axis)  *  410.0); 
sonarJLtie.pl »  p; 

sonar_line.p2  =  q; 

sonar_line_theta  =  orientation(p,  q); 

currcnt_poly  =  w->poly_list; 
for  (k  =  1;  k  <s  w-::^gree;  k++)  { 

cunent.vertex  =  current4)oly->vertex_list; 
for  (j  =  0;  j  <  current_poly->degree;  j-H-)  { 
pi «  current_vertex->point; 
ql  =  current_vertex->next>>point; 
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sonar_linc_thcta)); 

1)&& 

(HPI  -  0.26))) 


sin(sonar_theta)))  { 
sin(sonar_theta)); 


sin(sonar_thcta)))  { 
sm(sonar_theta)); 


wall.pl  s  pi; 
wall.p2  »  ql; 

wall.theta  »  orientation(pl,  ql); 
sonar.theta  »  fabs(normalize(wall_theta 

if  ((segfnent_crossing_test(&wall,  &sonar_line)  » 

(sonar.theta  <  (HPI  ->■  0.26))  &&  (sonar.theta  > 


/* 

*  wall  and  sonar  beam  must 

*  intersect  at  90  +-  15 

*  degrees 

*/ 

{ 

if  (wall_theta  =  0.0  II  wall_theta  =  PI)  ( 
if  (sonar_table[n].d  >  fabs((p.y  -  pl.y)  / 

sonar_table[n].d  =  fabs((p.y  -  pl.y)  / 

sonar_table[n].update  « 1; 

) 

}; 

if  (wall_theta  ==  -HPI  II  wall.theta  =  HPI)  { 
if  (sonar_table[n].d  >  fabs((p.x  -  pl.x)  / 

sonar_table[n].d  =  fabs((p.x  •  pl.x)  / 

sonar_table[n].update  =1;) 

}; 

} 

current_veifex  =  cunent_vertex->ncxt; 

/♦  end  for  each  vertex  loop  */ 


}  /*  end  for  each  polygon  loop  */ 

}  /*  end  if  sonar  is  enabled  test 

}  /*  end  outer  for  each  sonar  in  group  loop  */ 

/* 

*printf(“%s%d%s%2.21f«’V’Sonar“.n”  Range  is  =>“, 

♦  sonar_table[n].d); 

*/ 

if  (scrvicc_flag  !=  0) 

for  (i  =  0;  i  <  16;  i++)  { 
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if  (sonar.taUe[i].update  l)  { 

if  (sonar_tabIe[i].global » 1) 
calculalB_glotMl(i); 
if  (soiiar_tabl^i].fitting  » 1) 
linear.fitdngO); 

if  (sonar.table(i].filetype[0]  >«  1) 

log_data(i,  1,  soiiar_table[i].filenuiiri)er[0],  0); 
if  (sonar_tabIe[i].filetype[l] » l) 

lo^dau^i.  2,  sonar_table[i].fflenunibeitl],  0); 
)  /•end  if*/ 

sonar_table(i].update  «  0; 

}  /*  end  for  each  sonar  updated  •/ 

}  /*  end  serve_sonar  •/ 

#endif 

#endif 
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/^declaration  of  functions  and  return  values*/ 

/*sonarmath.h  */ 

extern  double  sonar(); 

extern  void  linear_fittingO; 

extern  posit  globalQ; 

extern  LINE_SEG  ♦get.segmentQ; 

extern  void  calculate_globalO; 

extern  void  add_to_segmentO: 

extern  void  goierate.segmentO; 

extern  LINE_SEG  *get_cuiTent^segmentO; 

extern  LINE_SEG  *end_segment(); 

extern  void  initialire.sonarO: 


/*sonannath.c  */ 

#include  “minl.h** 

void 

initialize_sonar() 

{ 

int  i,  k; 

/*  initialize  sonar.table  and  segment.data  */ 


for(i  =  0;i<16;i++){ 

sonar_table[i].global »  0; 
sonar_table[i].fitting  =  0; 
sonar_table[i].Eletype[0]  =  0 
sonar_table[i].nietype[l] 
sonar_table[i].filetype[2]  =  0 
sonar_table[i].update  »  0; 
sonar_table[ij.d  =  0.0; 
sonar_table[i].x  =  0.0; 
sonar_table[i].y  *  0.0; 
sonar_table[i].t  =  0.0; 

segment_data[i].alpha  =  0.0; 
segment_data[i].r  »  0.0; 
segnient_data[i].startx  =  0.0; 
segment_data[i].starty  «  0.0; 
segment_data[i].endx  »  0.0; 
segnient_data[i].endy  =  0.0; 
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enabled_sonars[i] »  0; 
seginent_data[i].n  =  0; 

) 

I*  set  up  compensation  for  sonar  position  */ 

sonar_table[0].rob_t  =  0.0; 
sonar_table[l].rob_t  =  3.142; 
sonar_table[2].rob_t  =  3.142; 
sonar_table[3].rob_t  =  0.0; 
sonar_table[4].rob_t  =  1.571; 
sonar_table[5].rob_t  =  1.571; 
sonar_table[6].rob_t  =  -1.571; 
sonar_table[7].rob_t  =  -1.571; 
sonar_table[8].rob_t  =  2.356; 
sonar_table[9].rob_t  =  -2.356; 
sonar_table[10].rob_t  =  -0.785; 
sonar_table[l  l].rob_t  =  0.785; 
sonar_table[12].rob_t  =  0.0; 
sonar_table[13].rob_t=  1.5708; 
sonar_table[  14].rob_t  =  4.7 124; 
sonar_table[15].rob_t  *  0.0; 

sonar_table[0].rob_x  =  18.0; 
sonar_table[ll.rob_x  =  -18.0; 
sonar_table[2].rob_x  =  -18.0; 
sonar_table[3].rob_x  =  18.0; 
sonar_table[4].rob_x  =  9.5; 
sonar_table[5].rob_x  =  -9.5; 
sonar_table[6].rob_x  =  -9.5; 
sonar_table[7].rob_x  =  9.5; 
sonar_table[8].rob_x  =  -15.0; 
sonar_table[9].rob_x  =  -14.5; 
sonar_table[10].rob_x=  15.5; 
sonar_table[l  l].rob_x  =  16.0; 
sonar_table[12].rob_x  =  0.0; 
sonar_table[131.rob_x=  1.5708; 
sonar_table[14].rob_x  =  4.7124; 
sonar_table[15].rob_x  =  0.0; 

sonar_table[0].rob_y  =  9.5; 
sonar_table[l].rob_y  =  9.5; 
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sonar_tablc[2].rob_y  « -9.5; 
soiiar_table{3].rob_y  =  -9.5; 
sonar_tablc[4].rob_y  =  19.75; 
sonar_tablc[5].rob_y  =  19.75; 
sonar_table[6].rob_y  =  -19.75; 
sonar_tablc[7].rob_y  =  -19.75; 
sonar_table[8i.rob_y  =  19.75; 
sonar_table[9].rob_y  =  -19.75; 
sonar_tablcI10].rob_y  =  -19.75; 
sonar_tablc[ll].rob_y  =  19.75; 
sonar_tablc[12]  Job_y  =  0.0; 
sonar_tablc[13]  Job_y  =  21.5; 
sonar_table(14]job_y  =  21.5; 
sonar_tablc[15].rob_y  =  0.0; 


group_array[01[0]  =  0; 
group_array[0][l]  =  5; 
group_aiTay[0][2j  =  2; 
group_airay[0][31  =  7; 
group_aiTay[l][0]  =  3; 
group_airay[l][l]  =  4; 
group_aiTay[l][2]  =  1; 
group_array[lj[3i  =  6; 
group_array[2j[0j  *  10; 
group_an’ay[2][l]  =  11; 
group_array[2i[21  =  8; 
group_array(2][3i  =  9; 
group_array[3][0i  =  12; 
group_aiTay[3][l]  =  13; 
group_array[3][2j  =  14; 
group_aiTay[3][3i  =  15; 


servicc_flag  =  0; 
Cl  =0.02; 

C2  =  5.0; 


} 


*  Procedure:  sonar(n) 
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*  Description:  returns  the  distance  (in 

*  centimeters)  sensed  by  the  n_th  ultrasonic  sensor.  If  no  echo  is 

*  received,  then  INFINITY(1.0e6)  is  returned.  If  the  distance  is  less  dian  10 

*  cm,  then  a  0  is  returned. 


double 
sonar(n) 
int  n; 

{ 

return  sonar_tablc[nl.d; 

} 


^**iif********^t****************************************************** 

*  Procedure:  global(n) 

Description:  returns  a  structure  of  type 

*  posit  containing  the  global  x  and  y  coordinates  of  the  position  of 

*  the  last  sonar  return. 

***m*************************m*************************************f 


posit  global  (n) 
int  n; 

{ 

posit  answer, 

if  (sonar_table[n].global  ==  0) 
calculat6_global(n); 
answer.gx  =  sonar_table[n].gx; 
answer.gy  =  sonar_table[n].gy; 
return  answer; 

} 


*  Procedure:  get_segment(n) 

*  Description:  returns  a  pointer  to  the 

*  oldest  segment  on  the  Unked  list  of  segments  for  sonar  n;  i.e.  the 

*  record  at  the  head  of  the  linked  list  It  is  destructive,  thus 

*  subsequent  calls  will  return  subsequent  segments  until  the  list  is 

*  empty.  This  is  accomplished  by  first  copying  the  contents  of  the 

*  head  record  into  a  temporary  record  called  segstruct  and  then 

*  freeing  the  allocated  memory  for  the  head  record.  The  pointer 

*  returned  is  actually  a  pointer  to  this  temporary  storage.  If 

*  get.segment  is  called  on  an  empty  list  a  null  pointer  is  returned. 
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4I******************************************************************/ 


UNE.SEG  * 
get_seginent(n) 
int  n; 

( 

LINE.SEG  *ptn 
int  index; 


index  =  seg_list_head[n]; 
if  (index  =  -1) 

ptr  =  NULL; 

else  { 

ptr  =  &seg_list[n][index]; 

seg_list_head[n]  =  (index  <  4)  ?  (index  +  1) :  0; 

} 

return  ptr, 

} 


^*4i*«*)ti*****4i4i**4i*****4i*4i*********4i*i|i****4i***4i*****«**«*4i4i**«****** 

*  Procedure:  end_scgment(n) 

*  Description:  this  procedure  allocates 

*  memory  for  the  segment  data  structure,  loads  the  correct  values 

*  into  it  and  returns  a  pointa  to  the  stractuie. 

**1,0,^t************************>t>**^HI‘*******************************/ 


LINE.SEG  * 

end_segment(n) 
int  n; 

{ 

LINE_SEG  *seg4>tr; 

double  startx,  starty,  endx,  endy,  delta,  alpha,  r,  length; 

seg_ptr  =  &segstruct; 

startx  =  segment_data{n].startx; 
starty  =  segment_data[n].starty; 
endx  =  segment_data[n].endx; 
endy  =  segment_data[n].endy; 
alpha  =  segment_data[n].alpha; 
r  =  segment_data[n].r, 

delta  =  startx  *  cos(alpha)  +  starty  *  sin(alpha)  -  r, 
startx  =  startx  -  (delta  **  cos(alpha)); 
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staity  s  starty  -  (delta  *  sin(alpha)); 

delta  s  endx  *  cos(alpha)  endy  *  siii(alpha)  -  r, 

endx  «  endx  >  (delta  *  cos(alpha)); 

endy  endy  •  (delta  *  sin(alpha)); 

length  =  sqit(SQR(startx  •  endx)  +  SQR(starty  -  endy)); 


seg  ptr->headx  »  startx; 
seg  ptr->heady  =  staity; 
seg  ptr->tailx  =  endx; 
seg_ptr->taily  =  endy; 
seg  ptr->alpha  s  alpha; 
seg_ptr->r  =  r, 
seg  ptr->length  =  length; 
segj)tr->sonar  =  n; 

return  seg_ptr, 

} 


*  Procedure:  get_current_seginent(n) 

*  Description:  returns  a  pointer 

*  to  the  segment  currently  under  construction  if  there  is  one, 

*  otherwise  returns  null  pointer.This  is  accomplished  by  calling 

*  end.segment,  copying  the  data  into  segstruct  and  then  returning  a 

*  pointer  to  segstruct  The  memory  allocated  by  end.segment  is  then 

*  freed. 

4i4i*4i**4i)ii***4i*4i4i«4i**4i4i«4i*4i«********************4i4i*******4i*4i**4i*«****/ 


LINE.SEG  ♦ 
get_currait_segment(n) 
int  n; 

{ 

LINE.SEG  ♦ptr. 


ptr  =  end_segment(n); 
return  ptr, 

) 


^4>4i4i**4i***4>**4>*4i4i4i****4>**4i4i**4i4t4i4<4t*4i4i***4i***4i*)ii***4i4i)|i*4i**4i*i|t**4t***4i 


*  Procedure:  calculate_global(n) 

*  Description:  this  procedure 
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*  calculates  the  global  x  and  y  coordinates  for  the  range  value  and 

*  robot  configuration  in  the  sonar  table.  The  results  are  sttned  in 

*  the  sonar  table. 

^*m******^*********************************************************/ 


void 

calculate_global(n) 
int  n; 

{ 

double  lx,  ly.  It,  range,  rob_t,  rob.x,  rob_y; 
CONHGURATION  global; 
range  =  sonar_table[n].d; 
if  (range  >=  INFINTTYO)  { 

sonar_table[n].gx  » INFINITY; 
sonar_table[n].gy  =  INFINITY; 

}  else  { 


rob_x  =  sonar_table(n].rob_x; 
rob_y  =  sonar_table(n].rob_y; 
rob_t  =  sonar_table(n].rob_t; 

get_rob(K&global); 


/*  vehicle  compose  sonar  ♦/ 

lx  =  global.  X  +  (cos(global.t)  ♦rob_x)  -  (rob_y  *  sin(global.t)); 
ly  =  global,  y  +  (sin(global.t)  ♦rob.x)  +  (rob_y  ♦  cos(global.t)); 
It  =  rob_t  +  global.t; 


vehicle  compose  sonar  range  */ 
sonar_table[n].gx  *  lx  +  (cos(lt)  *  range); 
sonar_table[n].gy  =  ly  +  (sin(lt)  *  range); 

} 

} 


^4i***«4i«*******4i4i*4i«**4i****4i*4>4>**«*****4i*4i*******4t********4t*4>**4t4>4i4i 

*  Procedure:  add_to_segment(n,  x,  y)  ♦  Description:  this  procedure 

*  calculates  new  interim  data  for  the  line  segment  and  stores  it  in 

*  segment_data[n].  It  also  changes  the  end  point  values  to  the  point 

*  being  added. 

4i4i**4i***4i4>4i4i**4i4i**4i4i4i4i**4i)|i*4i4i***4i**4i«4i«*4i*«4i*4i4i4i4>*4i4ti|i4i4i4i4i4i«4i*4i4i4i4i«y 


void 
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add_to_segnieiit(n,  x,  y) 
int  n; 
double  X,  y; 

{ 

double  mOO,  mlO,  mOl.  iti20,  ml  1,  iii02: 

double  alpha,  r; 

double  mux,  muy,  mm20,  mml  1,  mm02; 

mOO  s  segment_data[n].inOO  +=  1.0; 
mlO  s  segment_data[n].mlO  +=  x; 
mOl «  segment_data[n].m01  -m:  y; 
m20  -  segmait_data[n].m20  -f*  sqr(x); 
mil  =  segment_data[n].mll  -fs  x  *  y; 
m02  s  segment_data[n].m02  sq^y); 

if(m00<1.5){ 

segment_data[ii].staitx  »  x; 
segment_data(nj.starty  »  y; 

} 

mux  s  mlO  /  mOO; 

muy  s  mOl  /  mOO; 

mm20  =  m20  -  sqr(mlO)  /  mOO; 

mml  1  =  ml  1  -  mlO  *  mOl  /  mOO; 

mm02  -  m02  >  sqr(m01)  /  mOO; 

if(m00>1.5){ 

alpha  s  atan2(-2.0  *  mml  1,  (mm02  -  mm20))  /  2.0; 
r  =  mux  *  cos(alpha)  -i-  muy  *  sin(alpha); 

segment_data[n].alpha  =  alpha; 
seginent_data(n].r  - 
scgniMit_data[nl. ‘nr)  ;■  x; 

segment_data[n].enci>  »  y; 

) 

} 


^*****4i«******4i*******«**4i4i*4t***«**************4i**********«****** 

*'  Procedure:  ieset_moinents(n); 

*  Description:  resets  the  accumulative 

*  values  in  segmait_data[n]  /*  (m00,ml0,m01,m20,ml  1^002)  to  zero. 


*4i4i*4i**4i***«*4i*4i«*4i********4i«4i4i***«**«**4i****4t**4i4t**4i**4i4i4t**4i*4i****««^ 
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void 

reset_inoments(n) 
int  n; 

{ 

seginent_data(n].inOO  «  0.0; 
seginait_data{n].mlO  0.0; 
seginient_data(n].in01 »  0.0; 
seginrat_data[n].in20  «  0.0; 
segiiient_data[n].ml  1 »  0.0; 
segment_data[n].ni02  >  0.0; 

} 


y******************************************************************* 

*  Procedure:  geneTate_segment(n) 

*  Desorption:  this  function 

*  conpletes  segments  at  the  end  of  a  data  run.  Necessary  because  the 

*  linear  fitting  function  only  tenninates  a  segment  based  on  the  data 

*  -  it  has  no  way  of  knowing  that  the  user  has  stopped  collecting  data. 


void 

generate_segment(n) 
int  n; 

{ 

LD1E_SEG  *seg_ptr; 
if  (segfnent_data[n].m00  >  10.0)  { 
seg_ptr  =  end_segment(n); 
build  listfseg  ptr.  n); 

} 

reset_mofnents(n); 

} 


f*****m***********************************************m************* 

*  Procedure:  linear_fitting(n) 

*  Revised  by  Y.  Kanayama,07-07-93 

*  Description:  this  procedure  controls  the  fitting  of  point 

*  data  to  straight  line  segments.  Hrst  it  tests  if  the  new  coming 

*  point  is  not  far  from  the  fitted  line.  If  the  test  is  passed,  the 

*  point  is  added  to  test  if  die  thinnes  test  is  passed.  If  it  is 

*  passed,  the  addition  is  finalized.  If  any  of  the  tests  fail, 

*  the  line  segment  is  ended  and  a  new  one  started.  The  completed  line 


*  segment  is  sieved  in  a  data  structure  called  segment,  and  segments 

*  are  linked  together  in  a  linked  list 

M*****************************************************************/ 


void 

linear_fitting(n) 
int  n; 

{ 

double  X,  y,  mOO; 

double  alpha,  r,  delta; 

double  sonar_range; 

LINE.SEG  *finished_segment; 
sonar.range  -  sonar_table[n].d; 

if  (sonar_range  <  9.3  II  sonarjrange  >  409.0)  { 
gaierate_segment(n); 
return; 

} 

X  =  sonar_table[n].gx;/*  temporary  moments  *! 

y  «  sonar_table[n].gy; 

mOO  s  segment_data[n].mOO; 

if(m00<1.5){ 

add.to_segment(n,  x,  y); 
return; 

) 

r  s  segment_data[n].r, 

alpha  s  segment_data[n].alpha; 

delta  s  fabs(r  -  x  *  cos(alpha)  -  y  *  sin(alpha)); 

if  (delta  >  max2(C2,  Cl  *  sonar_range))  { 
gena:ate_segment(n); 
add_to_segment(n,  x,  y); 
return; 

}  else  { 

add_to_segmcnt(n,  x,  y); 
return; 

} 


}  /*  end  linear^fitting  ♦/ 

^*41 4i*«4i4i4i4i*4i4i4t4i4i4i***4i4i4i4i*«4i**4i4i*4i*4i*****4i4i*4i4i4i4i4i*i|i4i4i«4t««4i4i4i  41*4141414141 
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*  Procedure:  build_list(ptr,  n); 

*  Description:  tins  function  accepts 

*  a  pointer  to  a  segment  data  structure  and  a  sonar  number,  and 

*  iqtpends  the  segment  structure  to  tiie  tail  of  a  linked  list  of 

*  structures  for  that  sonar. 

************•*****«*****•***************************************/ 

void 

build_list(ptr,  n) 
int  n; 

UNE.SEG  *ptr, 

{ 

int  next; 

if  (scg_list_tail[n] »» -1) 

scg_list_head[aj  =  0; 

next  ®  (seg_list_tail[n]  <  4)  ?  ++scg_list_tail[n] :  0; 
if  (next »  seg_list_head[n]) 

seg_list_head[n]  *  (seg_list_head[n]  <  4)  ?  ++scg_list_head[nl :  0; 
scg_list[n][next]  =  *ptr; 
if  (sonar_table[n].filetype[2]  ==  1) 

log_data(n,  3,  sonar_table[n].filaiumber[2],  next); 

) 
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I* 

Author  :  Patrick  Byrne 
Date  :  February  22, 1994 
File  :  sonario  Ji 

Description :  Provides  extern  declarations  for  functions  in  sonario.c 

•/ 

extern  void  xfer_raw_to_hostO; 
extern  void  xfer_global_to_hostO: 
extern  void  xfer_segnient_to_host(); 
extern  void  host_xfer0‘. 

/* 

*  Author  :  Patrick  Byrne 

*  Date  :  22  February  1994 
*FUe 

*  sonario.c 

*  Description  :  Provides  the  following  functions  for  sonar  io: 

*  host_xfcrO  xfcr_scgnicnt_to_ho«0  xfcr_raw_to_hostO  xfer_global_to_host() 

*/ 


#include  ‘*inml.h" 
#ifndefSIM 


*  Procedure:  xfer_raw_to_host(filenuinber,fUenafne) 

*  Description: 

*  this  foiK:tion  allocates  memory  for  a  buffer  and  then  converts  a  raw  data 

*  log  file  to  a  string  format  stored  in  the  buffer.  It  then  calls  host_xfer 

*  to  send  the  string  to  the  host  Whoi  that  transfer  is  con^lete,  it 

*  frees  the  memory  it  allocated  for  the  bu^er.  Hlename  must  be 

*  entered  in  double  quotes  ( “dumpraw”  for  example). 


void 

xfer_raw_to_host(fUenumber,  filename) 


int 

nienumber; 

char 

^filename; 

char 

♦rbuffer. 

char 

♦start; 
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int 


i,  c,j; 

i «  Taw_data.log[filenundier].next; 
c»20  +  (i*33); 
rbuffer  s  inalloc(c); 
start  «  rbuffer, 
for(j»0;j<i;j++)  { 

imnt_flex(raw_datau.log[filenumber].daiTay|j],  rbuffer); 
princflex(raw_data_log[filenumber].xatTayij],  rbuffer); 
print_flex(raw_data_log[filaiumber].yaiTay|j],  rbuffer); 
print_flex(raw_data_log[filenumber].tarray|j],  rbuffer); 
nl_flex(rbuffer); 

} 

putb(*NO’,  rbuffer); 
rbuffer  s  start; 
host_xfer(rbuffer,  filename); 
free(rbuffer); 


^****4i«4i4i4i***4i*«*****4i*«******4i***4i*****«*********4i******4i*4t****«* 

*  Procedure:  xfer_global_to_host(filenumber,fUename)  Description: 

*  this  function  performs  the  same  function  as  xfer.raw_to_host,  but  for 

*  global  data  vice  raw  data. 

*«4i4i******4i4i*4i4i***4i***4i***4i****4i***4i*******4i**4i*4i*4i4i********4i*4i*4i*4i****^ 


void 

xfer_global_to_host(filenumber,  filename) 


int 

filenumber. 

char 

'"filename; 

char 

'"gbufifcr; 

char 

■"start; 

int 

i.  c,j; 

i  global_data_log[filenumber].next; 
c  =  20  +  (i*17); 

/*c  =  20  +  (i*22);*/ 
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gbuffer  s  malloc(c); 


start  s  gbuffen 
for(j  *0;  j  <i;j++)  { 

print_flex(global_data_log[filenumba].xarray[j],  gbuffer); 
print_flex(global_data_log[filenumber].yaiTay|j],  gbuffer); 
nl_flex(gbuffer); 

} 

putb(‘V0’,  gbuffer); 
gbuffa  s  start; 
host_xfer(gbuffer,  filename); 
fieeCgbuffer); 


^4i4i****4i*4i4i4i«**4i4i4i**4i*4i*4i**4i4i4>4i*4i4i4t*4i*4i4i«*******«*****4i4i4i4i4i***4i4i*4i** 

*  Procedure:  xfer_segnient_to_host(fUenumber, filename) 

Description: 

*  this  function  performs  the  same  function  as  xfer_raw_to_host,  but  for 

*  segment  data  vice  raw  data. 


void 

xfer_segment_to_host(filenumber,  fil^iame) 
int  filenumber, 

char  ^filename; 

{ 

char  *segbuffer, 

char  *start; 

int  i,c,j; 


i  =  segment_data_log[filenumber].count; 

/*c  =  20  +  (i*77);*/ 
c  =  20  +  (i*85); 
segbuffer  :=  malloc(c); 
start  s  segbuffer; 
for  (j  =  0;  j  <  i;  j++)  { 

print_flex(segment_data_log[filenumber].arTay[j].headx,  segbuffer); 
print_flex(segment_data_log[nienumber].array(j].heady,  segbuffa); 
nl_flex(segbuffer); 

print_flex(segment_data_log[filenumber].array|j].tailx,  segbi^er); 
print_flex(segment_data_log[filenumber].array(j].taily,  segbuffer); 
nl_flex(segbuffer); 
nl_flex(segbuffer); 

} 
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pud)(*NO\  segbuffer); 
segboffer  «  start; 
host_xfer(segbaffer.  fUename); 
ficee(segbuffer); 

} 


^*«**4i«***4i«**************«***************************«*******«**** 

*  Procedure:  host_xfer(bu£fer^enaine) 

*  Description:  this  function 

*  transfers  a  data  string  from  the  buffer  to  the  host  Not  a  user 

*  function;  is  called  by  data  conversion  functions  such  as  xfer_raw_to_host 

*  User  would  call  the  xfer_raw.to_host  (or  equivalent  for  global  or 

*  segment  data)  to  download  data  from  the  robot 


*«4i*4i4i4i*4i4i*4i4i4i*4i4i4i*4i******4i**4i********«***4i«4i*************4<4i***********y 


void 

host_xfer(buffer,  filename^ 
char  *buffer; 

char  ^filename; 

{ 

i_port(HOST.  PORT.SPEED,  0, 0. 0); 

r_printfCM2M5  connect  cable  and  keyinV’V”'); 

while  (r_getchar()  N  ‘  ‘); 

putstr(‘>n”,  HOST); 

i_port(HOST,  PORT.SPEED,  0, 0, 1); 

rj)rintfCM2M5  ready  for  dump  “); 

while  (r_getchar()  !*  ‘g’); 

putstr(“ytof“,HOST); 

putstr(£ilename,  HOST); 

putstr(“w\n”,HOST); 

while  (r_getchar()  !=  ‘  ‘); 
r_printfC*dun9ing  “); 
putstr(buffer,  HOST); 
putb(M’,HOST); 

putb(M’,  HOST); 
r4)rintfC^AA7”); 
return; 

} 

#endif 
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/*  Includes  from  sonar  sub-modules  *! 
#include  “sonarmath.h" 

#include  ‘‘sonarcard.h” 

#include  “sonario.h” 

#include  “sonarlog.h” 


/^declaration  of  functions  and  return  values  for  sonarsys.c*/ 

extern  void  set_sonar_parameters(); 
extern  void  build_listO; 

extern  CONFIGURATION  get_sonar_config(); 

#include  “mml.h” 
fifdefSIM 

#include“/n/geinini/work2/yamabico/mml/Sim/spatial.h" 

#endif 


*  Procedure:  set_sonar_parameters(cl,c2) 

*  Description:  allows  tl^  user  to 

*  adjust  constants  which  control  the  linear  fitting  algorithm.  Cl  is 

*  a  multiplier  to  allow  more  lenancy  for  greater  sonar  ranges. 

*  C2  is  an  absolute  value;  both  are  used  to  determine  if  an 

*  individual  data  point  is  usable  for  the  algorithm.  Default  values 

*  are  set  in  main.c  to  .02,  S.O  respectively. 


void 

set_sonar_parameters(cl,  c2) 
double  cl; 

double  c2; 

{ 

Cl  =cl; 

C2  =  c2; 

} 


FUNCTION:  get_sonar_config() 
PARAMETERS: 

PURPOSE: 
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RETURNS: 


CALLED  BY: 

CALLS:  NONE 

COMMENTS:  1 1  Sq)tember  92  -  Dave  MacPherson 

****************m***********************************m***********y 

CONHGURATION 


gct_sonar_config(scg_count) 
int  seg_count: 

{ 

CONFIGURATION  Qsonar; 


Qsonar.x  =  segment_data_log[0].array[seg_count].tailx; 

Qsonar.y  =  segnient_data_log[Oj.array[seg_count].taily; 

Qsonar.t  -  atan2(segnient_data_log[0].aiTay[seg_count].heady  - 
segment_data_log[0].aiTay[seg_count].taily, 
seginent_data_log[0].array[seg_count].headx- 
segiTient_data_log[0].array[seg_count].tailx); 

Qsonar.k  =  0.0; 

return  Qsonar, 
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/* 

Author  :  Patrick  Byrne 
Date  :  22  Feburary  1994 
File  :  sonarlog.h 

Description :  Provides  extern  declarations  for  functions  in  sonarLog.c 

*l 


extern  void  enable_data_logging(); 
extern  void  disable.dataJoggingO; 
extern  log_dataO; 
extern  void  set_log_interval(); 

I* 

*  Author  :  Patrick  Byrne 

*  Date  :  22  February  1994 
File  :sonarlog.c 

*  Description  :  Provides  the  following  Sonar  Logging 
functions: 

* 

*  void  enable_data_logging();  void  disablc_dataJoggingO;  void  log_data(); 

*  void  set_log_intervalO; 

* 

*/ 

#include  “mml.h” 


/ 

*  Procedure:  enable_data_logging(n,filetype^enumber) 

*  Description:  causes  the  background  system  to  log  data  for  sonar  (n) 

*  to  a  nie  (Hlenumber).  The  diita  to  be  logged  is  q)ecified  by  an 

*  integer  flag  (filetype).  A  value  of  0  for  filetype  will  cause  raw 

*  sonar  data  to  be  saved,  1  will  save  global  x  and  y,  and  2  will  save 

*  line  segments.  The  filenumber  may  range  between  0  and  3  for  each  of 

*  the  three  types,  providing  up  to  12  data  files.  Example: 

*  enable_data_logging(4,0,0);  will  cause  raw  data  from  sonar  #4  to  be 

*  saved  to  file  0,  while:  enable_data_logging(7,2,0); 

*  will  cause  segments  for  sonar  #7  to  be  saved  to  frle  0.  Function 

*  increments  the  service_flag. 

*******************************************************************^ 
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void 

enable_data_logging(n,  fUetype,  fUenumber) 
int  n.  filetype,  filenumber, 

{ 

global_data_log[fUenumber].next  =  0; 
if  (filetype  =  1) 

sonar_table[n].global  =  1; 
sonar_table[n].filetype[filetype] » 1; 
sonaT_table[n].filenuinber[filetype]  =  filenumber, 
++service_flag; 

} 


*  Procedure:  disable_data_logging(n,filetype) 

Description:  causes  the  background  system  to  cease  logging  data  of  a 

*  given  Retype  for 

*  a  sonar  n.  Decrements  the  service.flag. 


void 

disable_data_logging(n,  filetype) 
int  n,  filetype; 

{ 

if  ((filetype  =  1)  &&  (sonar_table[n].fitting  =  0)) 
sonar_table[n].global  =  0; 
sonar_table[n].filetype[filetype] »  0; 

-service_flag; 

} 


^*)|i4i*4i*4i4i«4i4i*4i4i*4i4i4i4i4i4i4i4i4i4i4i4i4i4i4i«4i4i4i**4i:»**4i**4t4t**4t4i4t*4i*4i4i*4i*4i4i4i4i4i«4,4i 

*  Procedure:  log_data(n,  type,  filenumber,!) 

*  Description:  this 

*  procedure  causes  data  to  be  written  to  a  file.  The  filenumber 

*  designates  which  “column”  (0,1,2,  or  3)  of  a  two  dimensional  array  for 

*  that  type  of  data  is  used.  The  data  array  and  a  counter  for  each  colunm 

*  forms  the  data  structure  for  each  type.  The  value  of  i  is  used  to  index 

*  the  seg_list  array  for  storing  line  segments. 


log_data(n,  filetype,  filenumber,  i) 


int  n.  fUetype,  fiknumber,  i; 

{ 

int  count,  interval,  next; 


switch  (flletype)  { 
case  1: 

count  s  raw.data Jog[filaiumber].count; 
interval  =  sonar_table[n]  interval; 
if  ((count  <  MAXRAW)  &&  !(count  %  interval))  { 
next  s  raw_data_log(filenumber].next; 
raw_data_log[filenumber].darray[next] »  sonar_table[n].d; 
raw_data_log[filenumber].xarray[next] »  sonar_table(n].x; 
raw_data_log[filenuinber].yarray[next]  s  sonar_table[n].y; 
raw_data_log[fUenumber].tarTay[next]  =  sonar_table[n].t; 
raw_data_log[filenumber]jiext  +=  1; 

} 

raw_data_log[filenumber].count+s  1; 
break; 

case  2: 


if  (sonar_table[n].gx  =  INFINTTY)! 

next  =  global_data_log[filenumber].next; 
if  (global_data_log[filenumber].xarray[next-l]  <  9999){ 
count  =  global_data_log[filenumber].count; 
interval  =  sonar_table[n].interval; 
if  ((count  <  MAXOLOBAL)  &&  !(count  %  interval))  { 
next  s  global_data_log[filenumber].next; 
global_data_log[filenumber].xazTay[next]= 

INFINITY; 

global_data_log[fiIenumber].yaiTay[next]= 

INFINITY; 

global_data_log[filaiumber].next-fs  1; 

} 

global_data_log[fiIaiumber].count-i-s  1; 

} 

} 

else{ 

count  =  global_data_log[filaiumber].count; 
interval »  sonar_table[n].interval; 
if  ((count  <  MAXGLX)BAL)  &&  !(count  %  interval))  { 
next  =  global_data_log[filenumber].next; 
global_data_log[filenumber].xarray[next]s 

sonar_table[n].gx; 
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global_data_log[fileniiinber].yaiTay[next]s 

sonar_table[n].gy; 

global_data_log[filenumber].iiext*<«  1; 

} 

global_data_log[filenuinber].count  -hs  l; 

} 

break; 

case  3: 

count  =  segment_data_log[filenumber].count; 
if  (count  <  MAXSEGMEND  { 

segment_data^log[filaiumber].array[count]  =  seg.list[n][i]; 

#ifdefSIM 

printfC  VsnLogging  segment  data  count  »>  %d  sonar  «>  %d 
count,  n): 

printfC^nThe  Line  segment  is: 
%s%5.1f%s%5.1f%s%5.1f%s%5.1fw%s%5.1f%s%5.1iNn”, 

“headx  =  “, 

segment_data_log[filenumbn].array[count].headx, 

“  heady  =  “, 

segment_data_log[filenumba'].array[count].heady, 

“  taax  =  “, 

segment_data_log[filenumb(7].array[count].taib(, 

“  tally  =  “, 

segment_data_log[fiienumber].aiTay[count].taily, 

“  length  = 

segment_data_log[fUenumbCT].airay[count].length/* 

“  Phi 

segment_data_log[fUenumber].aiTay[count].phi*/); 

#endif 


segment_data_log[filenumba'].count  -hs  1; 
break; 


} 


Z****************************************************************** 

*  Procedure:  set_log_int»val(n,d) 

*  Description:  this  procedure 

*  allows  the  user  to  set  how  often  the  sonar  system  writes  data  to 
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*  the  raw  data  or  global  data  files.  The  interval  d  is  stored  at 

*  sonar_table[n],  and  one  data  point  will  be  recorded  for  every  d  data 

*  points  sensed  by  the  sonar.  Default  value  for  interval  d  is  13,  which  for 

*  a  speed  of  30  cm/sec  and  sonar  sampling  time  of  25  msec  should 

*  record  a  data  point  every  10  cm. 

void 

set_log_interval(n,  d) 
int  n,  d; 

{ 

sonar_table[n]  .interval  =  d; 

} 
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B.  USER  FILES 


#include  “nunl.h” 
user() 

{ 

I*  File  for  translation  scanning  of  sonar  4  */ 

/*  Uses  logging  functions  for  local  trace,  */ 

/*  segment  sonar  data,  and  global  sonar  data*/ 

/*  Pat  Byrne  llNov93  */ 

/♦Casel*/ 

CONFIGURATION  first,  second: 
double  s; 

def_configuration(lSOO.0, 146.0,  PI,  0.0,  &first); 
def_configuration(800.0, 0.0,  HPI,  0.0,  &second); 
s  =  20.0; 
speed(lS.O); 

buffer.loc  =  index.loc  =  inalloc(300000) ; 

bufloc  s  indxloc  « (double  *)  malloc(600(X)); 

loc_tron(2, 0x3f,  30); 

set_rob(&first); 

enable_sonar(RIGHTF); 

si2«_const(s); 

set_log_int«val(RIGHTF,  1); 

enable_linear_fitting(RIGHTF); 

enable_data_logging(RIGHTF,  1, 0); 

enable_dataJogging(RIGHTF,  2, 0); 

enable_display_statusO; 

line(&first); 

bline(&second); 

while(vehicle.x  >  1300.0); 

disablc_sonar(RIGHTF); 

disable_linear_fitting(RIGHTF); 

disable_dataJogging(RIGHTF,  2); 

disable_data_logging(RIGHTF,  1); 

loc_troff0; 

motor_on  =  NO; 

xfer_global_to_host(0,  “globalT.tcst”); 
xfer_seginent_to_host(0,  “scgm«it7.tcst”); 
loc_trduinpC‘loc_dunq).tcst”); 


} 
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#iiiclude  “iiiinl.h” 
userO 
{ 

t*  Hie  for  translation  scanning  of  sonar  7  */ 

/*  Uses  logging  functions  for  local  trace,  */ 

/*  segment  sonar  data,  and  global  sonar  data*/ 

/*  Pat  Byrne  31  Jan  93  */ 

/♦Case  2*? 

CONFIGURATION  first,  second,  third,fourth,fifth,sixth; 
double  s; 

def_configuration(0.0, 0.0, 0.0, 0.0,  &first); 
def_configuration( 100.0, 300.0,  HPI,  0.0,  &second); 
s  a  20.0; 
speed(lS.O): 

buffer_loc  =  index_loc  « tnalloc(300000) ; 

bufloc  a  indxloc  a  (double  *)  malloc(60000); 

loc_tron(2, 0x3f,  30); 

set_rob(&first); 

enable_sonar(RIGHTF); 

size_const(s); 

set_log_interval(RIGHTF,  1); 

enable_linear.fitting(RIGHTP); 

enable_data_logging(RIGHTF,  1,0); 

enable_data_logging(RlGHTF,  2, 0); 

aiable_display_statusO; 

line(&first); 

line(&second); 

while(vehicle.y  <  150.0); 

disable_sonar(RIGHTF); 

disable_linear_fitting(RIGHTF); 

disable_data_logging(RIGHTF,  2); 

disable_data_logging(RIGHTF,  1); 

loc_troff0; 

motor.on  =  NO; 

xfer_globaLto_host(0,  “global7.test”); 
xfaLsegment.to.hostCO,  “scgnient7.test”); 
loc_trdumpC‘loc_dump.test”); 


} 
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#iiicliide  *‘nuni.h” 

usnO 

{ 

/*  File  for  translation  scanning  of  sonar  7  */ 

/*  Uses  logging  functions  f(»  local  trace,  */ 

/*  segmmt  sonar  data,  and  global  sonar  data*/ 

/*  Pat  Byrne  llNov93  */ 

/♦Case  3*/ 

CONFIGURATION  first,  second,  third4burth,fifth,sixth; 
double  s; 

def_c(mfiguration(951.6,  >30.0,  HPI,  0.0,  &first); 
def_configuration(9S1.6,  -30.0,  HPI/2, 0.0,  &sea>nd); 
s  «  20.0; 
speed(lS.O); 

buffer_loc  « index_loc  «  nuilloc(300000) ; 

bufioc  s  indxloc  » (double  ♦)  inalloc(600(X)); 

loc_tron(2, 0x3f,  30); 

set_rob(&first); 

enable_sonar(RIGHTF); 

size_const(s); 

setJog_interval(RIGHTF,  1); 
enable_lmear_fitting(RIGHTF); 
enable_data_logging(RIGHTF,  1, 0); 
enable_data_logging(RIGHTF,  2, 0); 
enable_display_statusO; 

line(&first); 

line(&second); 

while(v^cle.x  <  1150.0); 

disable_sonar(RIGHTF); 

disable_linear_fitting(RIGHTF); 

disab]e_data_logging(RIGHTF,  2); 

disab]e_data_logging(RIGH7F,  1); 

loc_troff(); 

motor_on  =  NO; 

xfer_global_to_host(0,  “globalT.test”); 
xfcr_segnient_to_host(0,  “segmentT.test”); 
loc_trduinpC‘loc_dump.test”); 


} 
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/*  File  f(M:  rotational  scanning  of  sonar  3  */ 

/•Pat  Byrne  Nov30. 93  •/ 

/•Case4  •/ 

#include  ‘*niml.h” 
userQ 
{ 

CONHGURATION  pi; 
void  initializeO; 
void  cleanupO; 

def_configuration( 1200.0, 146.0, 0.0, 0.0,  &pl); 

speed(lS.O); 

initialize(pl); 

set_rob(&pl); 

rotate(DPl); 

while(vehicle.t  <  PI ); 

cleanupO; 

} 

void  initialize(pl) 

{ 

double  s  s  20.0; 

buffer_loc  *  indexjoc  =  malloc(300000) ; 

bufloc  s  indxloc  s  (double  •)  malloc(6(XXX)); 

loc_tron(2, 0x3f,  30); 

setjob(&pl); 

enable_sonar(FRONTR); 

speed(lS.O); 

size_const(s); 

sct_log_interval(FRONTR,  1); 
enable_linear_fitting(FRONTR); 
enable_data_logging(FRONTR,  1, 0); 
enable_data_logging(FRONTR,  2, 0); 

} 

void  cleanupO 

{ 

disable_sonar(FRONTR); 
gena:ate_segment(FRONTR); 
disable_data_logging(FRONlll,  2); 
loc_trofifO; 

motor_on  =  NO; 

xfer_global_to_host(0,  “global.test”); 
xfer_segnient_to_host(0,  “segment3.test”); 
loc_trdumpC‘loc_dunip.test”); 

) 
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I*  Rle  f(v  rotational  scanning  of  sonar  3  */ 

/•Pat  Byrne  Nov30. 93  •/ 

#incli]de  ‘*mniLh” 

/•Cases  •/ 

user() 

( 

CONHGURATION  pi; 
void  initializeO; 
void  ckanupO; 

def_conngiiration<9S1.6,  -504.S.  0.0, 0.0,  &pl}: 

speed(lS.O); 

initialize(pl): 

set_rob(&pl); 

rotate(DPI); 

while(vehicle.t  <  PI ); 

cleanupO; 

} 

void  initialize(pl) 

{ 

double  s  -  20.0; 

buffer.loc  « indexjoc  =  nialloc(300000) ; 

bufloc  31  indxloc  =  (double  •)  nulloc(60000); 

loc_tron(2, 0x3f,  30); 

set_|ob(&pl); 

enable_sonar(FRONTR); 

speed(15.0); 

size_const(s); 

set_log_interval(FRONTR,  1); 
a)able_linear_fitting(FRONTR); 
enable_data_logging(FRONTR,  1, 0); 
enable_data_logging(FRONTR,  2, 0); 

} 

void  cleanupO 

{ 

disable_sonar(FRONTR); 
generate_segmait(FRONTR); 
disable_data_logging(FRONTR,  2); 
loc_troff0; 
motor.on  =  NO, 

xfer_global_to_host(0-  “global.test”); 
xfer_seginMit_to_host(0,  “segnient3.test”); 
loc_trdunn)C‘loc_duinp.test”); 

} 
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/*  Hie  for  rotational  scanning  of  sonar  3  *! 
/♦Pat  Byrne  Nov30, 93  */ 

/*Case6  */ 

#include  “ninil.h” 
user() 

{ 

CX)NnGURATION  pi; 
void  initializeO; 
void  cleanupO; 

def_configuration(781.0, 100.00,  PI,  0.0,  &pl); 

speed(lS.O); 

initialize(pl): 

set_rob<&pl); 

rotate(PI); 

while(vehicle.t  <  DPI ); 
cleanupO; 

} 

void  initialize(pl) 

{ 

double  s  =  20.0; 

bufferjoc  =  index_loc  =  nialloc(300000) ; 

bufloc  *  indxloc  =  (double  *)  malloc(60000); 

loc_tron(2, 0x3f,  30); 

set_rob(&pl); 

enable_sonar(FRONTR); 

speed(15.0); 

size_const(s); 

set_log_interval(FRONTR,  1); 
enable_linear_fitting(FRONTO); 
enable_data_logging(FRONTR,  1, 0); 
enable  data_logging(FRONTR,  2, 0); 

} 

void  cleanupO 

{ 

disable_sonar(HlONTR); 
generate_segntent(FRONTR); 
disable_data_logging(FRONTTl,  2); 

loc_troff0; 

motor_on  =  NO; 

xfcr_global_to_host(0,  “global.test”); 
xfer_segment_to_host(0,  “segment3.test”); 
loc_trdunipC‘loc_dunip.test”); 

} 
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#include 


userQ 

{ 

/*  File  for  translation  scanning  of  sonar  7  */ 

I*  Uses  logging  functions  for  local  trace,  */ 

/*  segment  sonar  data,  and  global  sonar  data*/ 

/*  Pat  Byrne  lFeb94  */ 

/♦Case?  */ 

CONFIGURATION  first,  second; 
double  s; 

def_configuration(0.0, 0.0, 0.0, 0.0,  &first); 
def_conBguration(800.0, 0.0,  HPI,  0.0,  &second); 
s  =  20.0; 
speed(15.0); 

bufferjoc  *  index.loc  =  malloc(300000) ; 

bufloc  =  indxloc  3:  (double  *)  nialloc(60(X)0); 

loc_tron(2, 0x3f,  30); 

set_rob(&first); 

enable_sonar(RIGHTF); 

size_const(s); 

set_log_intcrval(RIGHTF,  1); 
enable_linear_£itting(RIGHTF); 
enable_data_logging(RIGHTF,  1, 0); 
enable_data_logging(RIGHTF,  2, 0); 
enable.display.statusQ; 

line(&first); 
bline(&second); 
while(vehicle.x  <300.0); 
disable_sonar(RIGHTF); 
disable_linear_fitting(RIGHTF); 
disable_data_logging(RIGHTF,  2); 
disable_data_logging(RIGHTF,  1); 
loc_trofif0; 
motor_on  =  NO; 

xfer_global_to_host(0,  “globalV.test”); 
xfer_segment_to_host(0,“segnient7.test”); 

loc_trdumpCToc_dump.test”); 

} 
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#inciude  “mfnl.h” 
userO 
{ 

I*  File  for  rotational  scanning  of  sonar  3  *! 

I*  Uses  logging  functions  for  local  trace,  */ 

/*  segment  sonar  data,  and  global  sonar  dataV 
/♦Pat Byrne  llNov93  ♦/ 

/*  case  8  ♦/ 

CX)NFIGURATION  first,  second; 
double  s; 

def_configuration(lSOO.0, 146.0,  PI,  0.0,  &first); 
def_configuration(800.0, 0.0,  HPI,  0.0,  &second); 
s  *  20.0; 
speed(lS.O); 

bufferjoc  =  index_loc  =  malloc(300000) ; 

bufloc  =  indxloc  ==  (double  ♦)  malloc(600(X)); 

loc_tron(2, 0x3f,  30); 

set_rob(&first); 

enable_sonar(RIGHTF); 

size_const(s); 

set_log_interval(RIGHTF,  1); 

enable_linear_fitting(RIGHTF); 
enable_data_logging(RIGHTF,  1, 0); 
enable_data_logging(RIGHTF,  2, 0); 
enable_display_statusO; 
line(&first); 
bline(&second); 
while(vehicle.x  >  1300.0); 
disable_sonar(RIGHTF); 
disable_linear_Btting(RJGHTF); 
disable_data_logging(RIGHTF,  2); 
disable_data_logging(RIGHTF,  1); 
loc_troff0; 
motor_on  =  NO; 

xfer_globaLto_host(0,  “global7.test”); 
xfer_seginent_to_host(0,  “segment7.test”); 
loc.trdumpCIoc.dump.test”); 
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#incliide  “ininl.h*’ 

/*  obstacle  example  one  */ 
userO 
{ 

double  hitll; 

CX)NFIGURATION  pl,p3,positl; 
def_configunition(10S1.0, 146.0, 0.0, 0.0,  &pl); 
def_configuration( 1651 .0,46.0,0.0,0.0,  &p3); 
qpeed(lS.O); 

t^er Joe  s  index  Joe  =  malloc(300000) ; 
bufloc  a:  indxloc  s  (double  *)  malloc(6(XXX)); 
loc_tron(2, 0x3f,  30); 

setjrob(&pl); 
enable_sonar(FRONTR); 
set_lo^interval(FRONTO,  1); 
enable_dataJogging(FRONTR,  1,0); 
hitl  1 «  sonar(FRONTll); 
line(&pl); 

whUe(hitl  1 »  100.0  II  hitl  1  »  0.0 ){ 
hitll  s  sonar(FRONTR); 

} 

skipO; 

line(&p3); 

get_rob(K&positl); 

while(positl.x  <  13S1){ 

get_Tob(K&positl); 

} 

skipO; 

line(&pl); 

get_robO(&positl); 
while  ^sitl.x  <  16S1){ 
get.robCK&positl ); 

} 

disable_sonar(FRONTR); 
disable_dataJogging(FRONTR,  1); 
loc_troffO; 
stopOO; 

motor_on  *  OFF; 

xfer_global_to_host(0,  “global?.  AVOID”); 
loc_tidumpC‘loc_duiiip.AVOID”); 

} 
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#iiiclude  ’‘innil.h” 

/*  example  of  obstacle  avoidance  2  */ 
userO 

double  hitll; 

CONFIGURATION  pi,  p3.  p4,  start,  positl; 
def_configuration(0.0,0.0,0.0,0.0,  &stait); 
def_configuration(600.0, 0.0, 0.0, 0.0,  d^l); 
def_configuration(0.0,  -100.0, 0.0, 0.0,  ^3); 
speed(15.0); 

bufferjoc  » index.loc  « inalloc(300000); 
bufloc  s  indxloc  » (double  *)  fnalloc(600(X)); 
loc_tron(2, 0x3f,  30); 
set_rob(&start); 

enable_sonar(FRONTR); 
enable_sonar^EFTF); 
hitl  1  =  sonar(FRONTR); 

line(&pl); 
hitll  s  9999.0; 

while  (hitl  1  >=  100.0  II  hitl  1  =  0.0)  { 
hitl  1  =  sonar(FRONTR); 

} 

skipO; 

lin^&p3); 

hitl  1  s  sonar(LEFTF); 
while  (hitl  1  >»  100.0  II  hitl  1 «  0.0){ 
hitl  1  =  sonar(LEFTF); 

) 

while  (hitl  1  <»  100.0  II  hitl  1 »  0.0){ 
hitl  1  =  sonar(LEFTF); 

I 

skipO; 
line(&pl); 
getjrobO(&positl ); 
while(positl.x  <  600.0)( 
get_robO(&positl ); 

} 

disabte_sonar(FRONTR); 

disable_.sonar(l£FTF); 
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) 


loc_troffO; 

sttqjOO; 

motor.on  »  OFF; 
loc_trduinpC‘loc_duiiip.AVOID”); 
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